[fbeV1] simple floating base estimation algorithm

CMakeLists.txt

@@ -0,0 +1,32 @@
+# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+# This software may be modified and distributed under the terms of the
+# GNU Lesser General Public License v2.1 or any later version.
+
+cmake_minimum_required(VERSION 3.5)
+project(whole-body-estimators LANGUAGES CXX
+                              VERSION 0.1)
+
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON)
+
+list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
+
+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)
+
+set(YARP_REQUIRED_VERSION 3.0.1)
+
+find_package(YARP ${YARP_REQUIRED_VERSION} REQUIRED)
+
+add_subdirectory(devices)
+
+include(AddUninstallTarget)
+

README.md

@@ -1,2 +1,54 @@
 # whole-body-estimators
 YARP-based estimators for humanoid robots.
+
+# Overview
+ - [:orange_book: Implementation](#orange_book-implementation)
+ - [:page_facing_up: Dependencies](#page_facing_up-dependencies)
+ - [:hammer: Build the suite](#hammer-build-the-suite)
+
+# :orange_book: Implementation
+The current implementations available in the `devices` folder include,
+- `baseEstimatorV1` includes a simple humanoid floating base estimation algorithm fusing legged odometry and IMU attitude estimation, aimed to be used along side walking controllers.
+
+![Floating Base Estimation Algorithm V1](doc/resources/fbeV1.png)
+
+
+# :page_facing_up: Dependencies
+* [YARP](http://www.yarp.it/): to handle the comunication with the robot;
+* [iDynTree](https://github.com/robotology/idyntree/tree/devel): to setup the floating base estimation algorithm;
+* [ICUB](https://github.com/robotology/icub-main): to use the utilities like low pass filters from the `ctrLib` library
+* [Gazebo](http://gazebosim.org/): for the simulation (tested Gazebo 8 and 9).
+
+  ## Optional Dependencies
+  * [walking-controllers](https://github.com/robotology/walking-controllers): to test the floating base estimation along side walking controllers
+
+It must be noted that all of these dependencies can be directly installed together in one place using the [robotology-superbuild](https://github.com/robotology/robotology-superbuild).
+
+
+# :hammer: Build the suite
+## Linux
+
+```sh
+git clone https://github.com/robotology/whole-body-estimators.git
+cd whole-body-estimators
+mkdir build && cd build
+cmake ../
+make
+[sudo] make install
+```
+Notice: `sudo` is not necessary if you specify the `CMAKE_INSTALL_PREFIX`. In this case it is necessary to add in the `.bashrc` or `.bash_profile` the following lines:
+``` sh
+export WBDEstimator_INSTALL_DIR=/path/where/you/installed
+export YARP_DATA_DIRS=${YARP_DATA_DIRS}:${WBDEstimator_INSTALL_DIR}/share/yarp:${WBDEstimator_INSTALL_DIR}/lib/yarp
+```
+
+# Using the estimators
+- [`baseEstimatorV1`](devices/baseEstimatorV1/README.md) Please follow the documentation available here to run the floating base estimator.
+
+# Authors
+```
+Prashanth Ramadoss <prashanth.ramadoss@iit.it>
+Giulio Romualdi    <giulio.romualdi@iit.it>
+Silvio Traversaro  <silvio.traversaro@iit.it>
+Daniele Pucci      <daniele.pucci@iit.it>
+```

cmake/AddInstallRPATHSupport.cmake

@@ -0,0 +1,170 @@
+#.rst:
+# AddInstallRPATHSupport
+# ----------------------
+#
+# Add support to RPATH during installation to your project::
+#
+#   add_install_rpath_support([BIN_DIRS dir [dir]]
+#                             [LIB_DIRS dir [dir]]
+#                             [INSTALL_NAME_DIR [dir]]
+#                             [DEPENDS condition [condition]]
+#                             [USE_LINK_PATH])
+#
+# Normally (depending on the platform) when you install a shared
+# library you can either specify its absolute path as the install name,
+# or leave just the library name itself. In the former case the library
+# will be correctly linked during run time by all executables and other
+# shared libraries, but it must not change its install location. This
+# is often the case for libraries installed in the system default
+# library directory (e.g. ``/usr/lib``).
+# In the latter case, instead, the library can be moved anywhere in the
+# file system but at run time the dynamic linker must be able to find
+# it. This is often accomplished by setting environmental variables
+# (i.e. ``LD_LIBRARY_PATH`` on Linux).
+# This procedure is usually not desirable for two main reasons:
+#
+# - by setting the variable you are changing the default behaviour
+#   of the dynamic linker thus potentially breaking executables (not as
+#   destructive as ``LD_PRELOAD``)
+# - the variable will be used only by applications spawned by the shell
+#   and not by other processes.
+#
+# RPATH aims in solving the issues introduced by the second
+# installation method. Using run-path dependent libraries you can
+# create a directory structure containing executables and dependent
+# libraries that users can relocate without breaking it.
+# A run-path dependent library is a dependent library whose complete
+# install name is not known when the library is created.
+# Instead, the library specifies that the dynamic loader must resolve
+# the library’s install name when it loads the executable that depends
+# on the library. The executable or the other shared library will
+# hardcode in the binary itself the additional search directories
+# to be passed to the dynamic linker. This works great in conjunction
+# with relative paths.
+# This command will enable support to RPATH to your project.
+# It will enable the following things:
+#
+#  - If the project builds shared libraries it will generate a run-path
+#    enabled shared library, i.e. its install name will be resolved
+#    only at run time.
+#  - In all cases (building executables and/or shared libraries)
+#    dependent shared libraries with RPATH support will have their name
+#    resolved only at run time, by embedding the search path directly
+#    into the built binary.
+#
+# The command has the following parameters:
+#
+# Options:
+#  - ``USE_LINK_PATH``: if passed the command will automatically adds to
+#    the RPATH the path to all the dependent libraries.
+#
+# Arguments:
+#  - ``BIN_DIRS`` list of directories when the targets (executable and
+#    plugins) will be installed.
+#  - ``LIB_DIRS`` list of directories to be added to the RPATH. These
+#    directories will be added "relative" w.r.t. the ``BIN_DIRS`` and
+#    ``LIB_DIRS``.
+#  - ``INSTALL_NAME_DIR`` directory where the libraries will be installed.
+#    This variable will be used only if ``CMAKE_SKIP_RPATH`` or
+#    ``CMAKE_SKIP_INSTALL_RPATH`` is set to ``TRUE`` as it will set the
+#    ``INSTALL_NAME_DIR`` on all targets
+#  - ``DEPENDS`` list of conditions that should be ``TRUE`` to enable
+#    RPATH, for example ``FOO; NOT BAR``.
+#
+# Note: see https://gitlab.kitware.com/cmake/cmake/issues/16589 for further
+# details.
+
+#=======================================================================
+# Copyright 2014 Istituto Italiano di Tecnologia (IIT)
+# @author Francesco Romano <francesco.romano@iit.it>
+#
+# Distributed under the OSI-approved BSD License (the "License");
+# see accompanying file Copyright.txt for details.
+#
+# This software is distributed WITHOUT ANY WARRANTY; without even the
+# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+# See the License for more information.
+#=======================================================================
+# (To distribute this file outside of CMake, substitute the full
+# License text for the above reference.)
+
+
+include(CMakeParseArguments)
+
+
+function(ADD_INSTALL_RPATH_SUPPORT)
+
+  set(_options USE_LINK_PATH)
+  set(_oneValueArgs INSTALL_NAME_DIR)
+  set(_multiValueArgs BIN_DIRS
+                      LIB_DIRS
+                      DEPENDS)
+
+  cmake_parse_arguments(_ARS "${_options}"
+                             "${_oneValueArgs}"
+                             "${_multiValueArgs}"
+                             "${ARGN}")
+
+  # if either RPATH or INSTALL_RPATH is disabled
+  # and the INSTALL_NAME_DIR variable is set, then hardcode the install name
+  if(CMAKE_SKIP_RPATH OR CMAKE_SKIP_INSTALL_RPATH)
+    if(DEFINED _ARS_INSTALL_NAME_DIR)
+      set(CMAKE_INSTALL_NAME_DIR ${_ARS_INSTALL_NAME_DIR} PARENT_SCOPE)
+    endif()
+  endif()
+
+  if (CMAKE_SKIP_RPATH OR (CMAKE_SKIP_INSTALL_RPATH AND CMAKE_SKIP_BUILD_RPATH))
+    return()
+  endif()
+
+
+  set(_rpath_available 1)
+  if(DEFINED _ARS_DEPENDS)
+    foreach(_dep ${_ARS_DEPENDS})
+      string(REGEX REPLACE " +" ";" _dep "${_dep}")
+      if(NOT (${_dep}))
+        set(_rpath_available 0)
+      endif()
+    endforeach()
+  endif()
+
+  if(_rpath_available)
+
+    # Enable RPATH on OSX.
+    set(CMAKE_MACOSX_RPATH TRUE PARENT_SCOPE)
+
+    # Find system implicit lib directories
+    set(_system_lib_dirs ${CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES})
+    if(EXISTS "/etc/debian_version") # is this a debian system ?
+        if(CMAKE_LIBRARY_ARCHITECTURE)
+            list(APPEND _system_lib_dirs "/lib/${CMAKE_LIBRARY_ARCHITECTURE}"
+                                         "/usr/lib/${CMAKE_LIBRARY_ARCHITECTURE}")
+        endif()
+    endif()
+    # This is relative RPATH for libraries built in the same project
+    foreach(lib_dir ${_ARS_LIB_DIRS})
+      list(FIND _system_lib_dirs "${lib_dir}" isSystemDir)
+      if("${isSystemDir}" STREQUAL "-1")
+        foreach(bin_dir ${_ARS_LIB_DIRS} ${_ARS_BIN_DIRS})
+          file(RELATIVE_PATH _rel_path ${bin_dir} ${lib_dir})
+          if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
+            list(APPEND CMAKE_INSTALL_RPATH "@loader_path/${_rel_path}")
+          else()
+            list(APPEND CMAKE_INSTALL_RPATH "$ORIGIN/${_rel_path}")
+          endif()
+        endforeach()
+      endif()
+    endforeach()
+    if(NOT "${CMAKE_INSTALL_RPATH}" STREQUAL "")
+      list(REMOVE_DUPLICATES CMAKE_INSTALL_RPATH)
+    endif()
+    set(CMAKE_INSTALL_RPATH ${CMAKE_INSTALL_RPATH} PARENT_SCOPE)
+
+    # add the automatically determined parts of the RPATH
+    # which point to directories outside the build tree to the install RPATH
+    set(CMAKE_INSTALL_RPATH_USE_LINK_PATH ${_ARS_USE_LINK_PATH} PARENT_SCOPE)
+
+  endif()
+
+endfunction()
+

cmake/AddUninstallTarget.cmake

@@ -0,0 +1,102 @@
+#.rst:
+# AddUninstallTarget
+# ------------------
+#
+# Add the "uninstall" target for your project::
+#
+#   include(AddUninstallTarget)
+#
+#
+# will create a file ``cmake_uninstall.cmake`` in the build directory and add a
+# custom target ``uninstall`` (or ``UNINSTALL`` on Visual Studio and Xcode) that
+# will remove the files installed by your package (using
+# ``install_manifest.txt``).
+# See also
+# https://gitlab.kitware.com/cmake/community/wikis/FAQ#can-i-do-make-uninstall-with-cmake
+#
+# The :module:`AddUninstallTarget` module must be included in your main
+# ``CMakeLists.txt``. If included in a subdirectory it does nothing.
+# This allows you to use it safely in your main ``CMakeLists.txt`` and include
+# your project using ``add_subdirectory`` (for example when using it with
+# :cmake:module:`FetchContent`).
+#
+# If the ``uninstall`` target already exists, the module does nothing.
+
+#=============================================================================
+# Copyright 2008-2013 Kitware, Inc.
+# Copyright 2013 Istituto Italiano di Tecnologia (IIT)
+#   Authors: Daniele E. Domenichelli <daniele.domenichelli@iit.it>
+#
+# Distributed under the OSI-approved BSD License (the "License");
+# see accompanying file Copyright.txt for details.
+#
+# This software is distributed WITHOUT ANY WARRANTY; without even the
+# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+# See the License for more information.
+#=============================================================================
+# (To distribute this file outside of CMake, substitute the full
+#  License text for the above reference.)
+
+
+# AddUninstallTarget works only when included in the main CMakeLists.txt
+if(NOT "${CMAKE_CURRENT_BINARY_DIR}" STREQUAL "${CMAKE_BINARY_DIR}")
+  return()
+endif()
+
+# The name of the target is uppercase in MSVC and Xcode (for coherence with the
+# other standard targets)
+if("${CMAKE_GENERATOR}" MATCHES "^(Visual Studio|Xcode)")
+  set(_uninstall "UNINSTALL")
+else()
+  set(_uninstall "uninstall")
+endif()
+
+# If target is already defined don't do anything
+if(TARGET ${_uninstall})
+  return()
+endif()
+
+
+set(_filename cmake_uninstall.cmake)
+
+file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/${_filename}"
+"if(NOT EXISTS \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\")
+  message(WARNING \"Cannot find install manifest: \\\"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\\\"\")
+  return()
+endif()
+file(READ \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\" files)
+string(STRIP \"${files}\" files)
+string(REGEX REPLACE \"\\n\" \";\" files \"${files}\")
+list(REVERSE files)
+foreach(file ${files})
+  if(IS_SYMLINK \"$ENV{DESTDIR}${file}\" OR EXISTS \"$ENV{DESTDIR}${file}\")
+    message(STATUS \"Uninstalling: $ENV{DESTDIR}${file}\")
+    execute_process(
+      COMMAND ${CMAKE_COMMAND} -E remove \"$ENV{DESTDIR}${file}\"
+      OUTPUT_VARIABLE rm_out
+      RESULT_VARIABLE rm_retval)
+    if(NOT \"${rm_retval}\" EQUAL 0)
+      message(FATAL_ERROR \"Problem when removing \\\"$ENV{DESTDIR}${file}\\\"\")
+    endif()
+  else()
+    message(STATUS \"Not-found: $ENV{DESTDIR}${file}\")
+  endif()
+endforeach(file)
+")
+
+set(_desc "Uninstall the project...")
+if(CMAKE_GENERATOR STREQUAL "Unix Makefiles")
+  set(_comment COMMAND $\(CMAKE_COMMAND\) -E cmake_echo_color --switch=$\(COLOR\) --cyan "${_desc}")
+else()
+  set(_comment COMMENT "${_desc}")
+endif()
+add_custom_target(${_uninstall}
+                  ${_comment}
+                  COMMAND ${CMAKE_COMMAND} -P ${_filename}
+                  USES_TERMINAL
+                  BYPRODUCTS uninstall_byproduct
+                  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}")
+set_property(SOURCE uninstall_byproduct PROPERTY SYMBOLIC 1)
+
+set_property(TARGET ${_uninstall} PROPERTY FOLDER "CMakePredefinedTargets")
+

devices/CMakeLists.txt

@@ -0,0 +1,6 @@
+# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+# This software may be modified and distributed under the terms of the
+# GNU Lesser General Public License v2.1 or any later version.
+
+add_subdirectory(baseEstimatorV1)
+