Merge branch 'devel' into 'master'

After 2nd RAL submission

See merge request !17

.gitlab-ci.yml

-image: segaleran/ceres
+stages:
+  - license
+  - build_and_test
 
-before_script:
-  - ls
+############ YAML ANCHORS ############
+.preliminaries_template: &preliminaries_definition
+  ## Install ssh-agent if not already installed, it is required by Docker.
+  ## (change apt-get to yum if you use an RPM-based image)
+  - 'which ssh-agent || ( apt-get update -y && apt-get install openssh-client -y )'
+
+  ## Run ssh-agent (inside the build environment)
+  - eval $(ssh-agent -s)
+
+  ## Add the SSH key stored in SSH_PRIVATE_KEY variable to the agent store
+  ## We're using tr to fix line endings which makes ed25519 keys work
+  ## without extra base64 encoding.
+  ## https://gitlab.com/gitlab-examples/ssh-private-key/issues/1#note_48526556
+  - mkdir -p ~/.ssh
+  - chmod 700 ~/.ssh  
+  - echo "$SSH_PRIVATE_KEY" | tr -d '\r' | ssh-add - > /dev/null
+  # - echo "$SSH_KNOWN_HOSTS" > $HOME/.ssh/known_hosts
+  - ssh-keyscan -H -p 2202 gitlab.iri.upc.edu >> $HOME/.ssh/known_hosts
+
+  # update apt
   - apt-get update
-  - apt-get install -y build-essential cmake 
-
-# SPDLOG
-#  - apt-get install -y libspdlog-dev
-  - if [ -d spdlog ]; then
-  -   echo "directory exists" 
-  -   if [ "$(ls -A ./spdlog)" ]; then 
-  -     echo "directory not empty" 
-  -     cd spdlog
-  -     git pull
-  -   else 
-  -     echo "directory empty" 
-  -     git clone https://github.com/gabime/spdlog.git
-  -     cd spdlog
-  -   fi
+
+  # create 'ci_deps' folder (if not exists)
+  - mkdir -pv ci_deps
+
+.license_header_template: &license_header_definition
+  - cd $CI_PROJECT_DIR
+
+  # configure git
+  - export CI_NEW_BRANCH=ci_processing$RANDOM
+  - echo creating new temporary branch... $CI_NEW_BRANCH
+  - git config --global user.email "${CI_EMAIL}"
+  - git config --global user.name "${CI_USERNAME}"
+  - git checkout -b $CI_NEW_BRANCH # temporary branch
+
+  # license headers
+  - export CURRENT_YEAR=$( date +'%Y' )
+  - echo "current year:" ${CURRENT_YEAR}
+  - if [ -f license_header_${CURRENT_YEAR}.txt ]; then
+      # add license headers to new files
+  -   echo "File license_header_${CURRENT_YEAR}.txt already exists. License headers are assumed to be updated. Adding headers to new files..."
+  -   ./ci_deps/wolf/wolf_scripts/license_manager.sh --add --path=. --license-header=license_header_${CURRENT_YEAR}.txt --exclude=ci_deps
   - else
-  -   echo "directory inexistent" 
-  -   git clone https://github.com/gabime/spdlog.git
-  -   cd spdlog
+      # update license headers of all files
+  -   export PREV_YEAR=$(( CURRENT_YEAR-1 ))
+  -   echo "Creating new file license_header_${CURRENT_YEAR}.txt..."
+  -   git mv license_header_${PREV_YEAR}.txt license_header_${CURRENT_YEAR}.txt
+  -   sed -i "s/${PREV_YEAR}/${PREV_YEAR},${CURRENT_YEAR}/g" license_header_${CURRENT_YEAR}.txt
+  -   ./ci_deps/wolf/wolf_scripts/license_manager.sh --update --path=. --license-header=license_header_${CURRENT_YEAR}.txt --exclude=ci_deps
+  - fi
+
+  # push changes (if any)
+  - if git commit -a -m "[skip ci] license headers added or modified" ; then
+  -   git remote set-url --push origin "ssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git"
+  -   git push origin $CI_NEW_BRANCH:${CI_COMMIT_REF_NAME}
+  - else
+  -   echo "No changes, nothing to commit!"
+  - fi
+
+.install_wolf_template: &install_wolf_definition
+  - cd ${CI_PROJECT_DIR}/ci_deps
+  - if [ -d wolf ]; then
+  -   echo "directory wolf exists"
+  -   cd wolf
+  -   git checkout devel
+  -   git pull
+  -   git checkout $WOLF_CORE_BRANCH
+  - else
+  -   git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf.git
+  -   cd wolf
+  -   git checkout $WOLF_CORE_BRANCH
   - fi
-  - git fetch
-  - git checkout v0.17.0
   - mkdir -pv build
   - cd build
-  - ls
-  - cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" -DSPDLOG_BUILD_TESTING=OFF ..
+  - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_DEMOS=OFF -DBUILD_TESTS=OFF ..
+  - make -j$(nproc)
   - make install
-  - cd ../..
-
-# YAML
-#  - apt-get install -y libyaml-cpp-dev
-  - if [ -d yaml-cpp ]; then
-  -   echo "directory exists" 
-  -   if [ "$(ls -A ./yaml-cpp)" ]; then 
-  -     echo "directory not empty" 
-  -     cd yaml-cpp
-  -     git pull
-  -   else 
-  -     echo "directory empty" 
-  -     git clone https://github.com/jbeder/yaml-cpp.git
-  -     cd yaml-cpp
-  -   fi
+
+.install_wolfimu_template: &install_wolfimu_definition
+  - cd ${CI_PROJECT_DIR}/ci_deps
+  - if [ -d imu ]; then
+  -   echo "directory imu exists"
+  -   cd imu
+  -   git pull
   - else
-  -   echo "directory inexistent" 
-  -   git clone https://github.com/jbeder/yaml-cpp.git
-  -   cd yaml-cpp
+  -   git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/plugins/imu.git
+  -   cd imu
   - fi
   - mkdir -pv build
   - cd build
-  - ls
-  - cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" -DYAML_CPP_BUILD_TESTS=OFF ..
+  - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_TESTS=OFF ..
+  - make -j$(nproc)
+  - make install
+
+.build_and_test_template: &build_and_test_definition
+  - cd $CI_PROJECT_DIR
+  - mkdir -pv build
+  - cd build
+  - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON ..
+  - make -j$(nproc)
+  - ctest -j$(nproc)
   - make install
-  - cd ../..
 
-wolf_build_and_test:
-  stage: build
+############ LICENSE HEADERS ############
+license_headers:
+  stage: license
+  image: labrobotica/wolf_deps:16.04
+  cache:
+    - key: wolf-xenial
+      paths:
+      - ci_deps/wolf/
+  except:
+    - master
+  before_script:
+    - *preliminaries_definition
+    - *install_wolf_definition
+  script:
+    - *license_header_definition
+
+############ UBUNTU 16.04 TESTS ############
+build_and_test:xenial:
+  image: labrobotica/wolf_deps:16.04
+  stage: build_and_test
+  cache:
+    - key: wolf-xenial
+      paths:
+      - ci_deps/wolf/
+    - key: imu-xenial
+      paths:
+      - ci_deps/imu/
+  except:
+    - master
+  before_script:
+    - *preliminaries_definition
+    - *install_wolf_definition
+    - *install_wolfimu_definition
+    - ldconfig # update links (shared libraries)
+  script:
+    - *build_and_test_definition
+
+############ UBUNTU 18.04 TESTS ############
+build_and_test:bionic:
+  image: labrobotica/wolf_deps:18.04
+  stage: build_and_test
+  cache:
+    - key: wolf-bionic
+      paths:
+      - ci_deps/wolf/
+    - key: imu-bionic
+      paths:
+      - ci_deps/imu/
+  except:
+    - master
+  before_script:
+    - *preliminaries_definition
+    - *install_wolf_definition
+    - *install_wolfimu_definition
+    - ldconfig # update links (shared libraries)
+  script:
+    - *build_and_test_definition
+
+############ UBUNTU 20.04 TESTS ############
+build_and_test:focal:
+  image: labrobotica/wolf_deps:20.04
+  stage: build_and_test
+  cache:
+    - key: wolf-focal
+      paths:
+      - ci_deps/wolf/
+    - key: imu-focal
+      paths:
+      - ci_deps/imu/
   except:
     - master
+  before_script:
+    - *preliminaries_definition
+    - *install_wolf_definition
+    - *install_wolfimu_definition
+    - ldconfig # update links (shared libraries)
   script:
-    - mkdir -pv build
-    - cd build
-    - ls # we can check whether the directory was already full
-    - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON ..
-    - make -j$(nproc)
-    - ctest -j$(nproc)
-    - make install
+    - *build_and_test_definition

CMakeLists.txt

 # Pre-requisites about cmake itself
-CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
 
 if(COMMAND cmake_policy)
   cmake_policy(SET CMP0005 NEW)
@@ -8,11 +8,12 @@ endif(COMMAND cmake_policy)
 # MAC OSX RPATH
 SET(CMAKE_MACOSX_RPATH 1)
 
-
 # The project name
 PROJECT(bodydynamics)
 set(PLUGIN_NAME wolf${PROJECT_NAME})
 
+MESSAGE("Starting ${PROJECT_NAME} CMakeLists ...")
+
 SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
 SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
 SET(CMAKE_INSTALL_PREFIX /usr/local)
@@ -26,18 +27,14 @@ message(STATUS "Configured to compile in ${CMAKE_BUILD_TYPE} mode.")
 SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -D_REENTRANT")
 SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT")
 
-#Set compiler according C++11 support
+#Set compiler according C++14 support
 include(CheckCXXCompilerFlag)
-CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
-CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-if(COMPILER_SUPPORTS_CXX11)
-		message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++11 support.")
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
-elseif(COMPILER_SUPPORTS_CXX0X)
-		message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++0x support.")
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
+if(COMPILER_SUPPORTS_CXX14)
+		message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++14 support.")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
 else()
-  message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+  message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.")
 endif()
 
 if(UNIX)
@@ -46,9 +43,18 @@ if(UNIX)
     "${CMAKE_CXX_FLAGS} -Werror=all -Werror=extra -Wno-unknown-pragmas -Wno-sign-compare -Wno-unused-parameter -Wno-missing-field-initializers")
 endif(UNIX)
 
+IF(NOT BUILD_TESTS)
+  OPTION(BUILD_TESTS "Build Unit tests" ON)
+ENDIF(NOT BUILD_TESTS)
+
+IF(NOT BUILD_DEMOS)
+  OPTION(BUILD_DEMOS "Build demos in demos folder, requires robotpkg-multicontact-api and robotpkg-pinocchio packages" OFF)
+ENDIF(NOT BUILD_DEMOS)
+
+IF(NOT BUILD_DOC)
+  OPTION(BUILD_DOC "Build Documentation" OFF)
+ENDIF(NOT BUILD_DOC)
 
-#OPTION(BUILD_DOC "Build Documentation" OFF)
-OPTION(BUILD_TESTS "Build Unit tests" ON)
 #############
 ## Testing ##
 #############
@@ -61,42 +67,24 @@ if(BUILD_TESTS)
     enable_testing()
 endif()
 
-#+START_SRC --------------------------------------------------------------------------------------------------------------------------------
-#Start WOLF build
-MESSAGE("Starting WOLF CMakeLists ...")
-CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
-
 #CMAKE modules
-
 SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
 MESSAGE(STATUS ${CMAKE_MODULE_PATH})
 
 # Some wolf compilation options
-
 IF((CMAKE_BUILD_TYPE MATCHES DEBUG) OR (CMAKE_BUILD_TYPE MATCHES debug) OR (CMAKE_BUILD_TYPE MATCHES Debug))
   set(_WOLF_DEBUG true)
 ENDIF()
 
 option(_WOLF_TRACE "Enable wolf tracing macro" ON)
 
-# option(BUILD_EXAMPLES "Build examples" OFF)
-set(BUILD_TESTS true)
-set(BUILD_EXAMPLES false)
-
-# Does this has any other interest
-# but for the examples ?
-# yes, for the tests !
-IF(BUILD_EXAMPLES OR BUILD_TESTS)
-  string(TOUPPER ${PROJECT_NAME} UPPER_NAME)
-  set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
-ENDIF(BUILD_EXAMPLES OR BUILD_TESTS)
-
-message("UPPER_NAME ${UPPER_NAME}")
-
+# ============ DEPENDENCIES  ============
+FIND_PACKAGE(wolfcore REQUIRED)
+FIND_PACKAGE(wolfimu REQUIRED)
 
-#find dependencies.
-FIND_PACKAGE(wolf REQUIRED)
-FIND_PACKAGE(wolfIMU REQUIRED)
+# ============ CONFIG.H ============ 
+string(TOUPPER ${PROJECT_NAME} UPPER_NAME)
+set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
 
 # Define the directory where will be the configured config.h
 SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/${PROJECT_NAME}/internal)
@@ -112,124 +100,93 @@ IF(EXISTS "${WOLF_CONFIG_DIR}" AND NOT IS_DIRECTORY "${WOLF_CONFIG_DIR}")
 ENDIF()
 # Configure config.h
 configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
-message("WOLF CONFIG ${WOLF_CONFIG_DIR}/config.h")
-message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
 include_directories("${PROJECT_BINARY_DIR}/conf")
 
-include_directories("include")
-include_directories(${wolf_INCLUDE_DIRS})
+# ============ INCLUDES ============ 
+INCLUDE_DIRECTORIES(${wolfimu_INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(BEFORE "include")
 
-#HEADERS
-
-SET(HDRS_COMMON
-  )
+# ============ HEADERS ============ 
 SET(HDRS_MATH
 include/bodydynamics/math/force_torque_delta_tools.h
   )
-SET(HDRS_UTILS
-  )
-SET(HDRS_PROBLEM
-  )
-SET(HDRS_HARDWARE
-  )
-SET(HDRS_TRAJECTORY
-  )
-SET(HDRS_MAP
-  )
-SET(HDRS_FRAME
-  )
-SET(HDRS_STATE_BLOCK
-  )
 SET(HDRS_CAPTURE
+include/bodydynamics/capture/capture_force_torque_preint.h
+include/bodydynamics/capture/capture_inertial_kinematics.h
+include/bodydynamics/capture/capture_leg_odom.h
+include/bodydynamics/capture/capture_point_feet_nomove.h
   )
 SET(HDRS_FACTOR
-include/bodydynamics/factor/factor_autodiff_inertial_kinematics.h
 include/bodydynamics/factor/factor_force_torque.h
+include/bodydynamics/factor/factor_force_torque_preint.h
+include/bodydynamics/factor/factor_inertial_kinematics.h
+include/bodydynamics/factor/factor_point_feet_nomove.h
   )
 SET(HDRS_FEATURE
-include/bodydynamics/feature/feature_inertial_kinematics.h
 include/bodydynamics/feature/feature_force_torque.h
-  )
-SET(HDRS_LANDMARK
+include/bodydynamics/feature/feature_force_torque_preint.h
+include/bodydynamics/feature/feature_inertial_kinematics.h
   )
 SET(HDRS_PROCESSOR
+include/bodydynamics/processor/processor_force_torque_preint.h
+include/bodydynamics/processor/processor_inertial_kinematics.h
+include/bodydynamics/processor/processor_point_feet_nomove.h
   )
 SET(HDRS_SENSOR
-  )
-SET(HDRS_SOLVER
-  )
-SET(HDRS_DTASSC
-  )
-SET(HDRS_YAML
+include/bodydynamics/sensor/sensor_force_torque.h
+include/bodydynamics/sensor/sensor_inertial_kinematics.h
+include/bodydynamics/sensor/sensor_point_feet_nomove.h
   )
 
-#SOURCES
-SET(SRCS_PROBLEM
-  )
-SET(SRCS_HARDWARE
-  )
-SET(SRCS_TRAJECTORY
-  )
-SET(SRCS_MAP
-  )
-SET(SRCS_FRAME
-  )
-SET(SRCS_STATE_BLOCK
-  )
-SET(SRCS_COMMON
-  )
-SET(SRCS_MATH
-  )
-SET(SRCS_UTILS
-  )
+# ============ SOURCES ============ 
 SET(SRCS_CAPTURE
-  )
-SET(SRCS_FACTOR
-  )
+src/capture/capture_force_torque_preint.cpp
+src/capture/capture_inertial_kinematics.cpp
+src/capture/capture_leg_odom.cpp
+src/capture/capture_point_feet_nomove.cpp
+)
 SET(SRCS_FEATURE
-src/feature/feature_inertial_kinematics.cpp
 src/feature/feature_force_torque.cpp
-  )
-SET(SRCS_LANDMARK
+src/feature/feature_force_torque_preint.cpp
+src/feature/feature_inertial_kinematics.cpp
   )
 SET(SRCS_PROCESSOR
-  )
+src/processor/processor_force_torque_preint.cpp
+src/processor/processor_inertial_kinematics.cpp
+src/processor/processor_point_feet_nomove.cpp
+)
 SET(SRCS_SENSOR
-  )
-SET(SRCS_DTASSC
-  )
-SET(SRCS_SOLVER
-  )
-SET(SRCS_YAML
+src/sensor/sensor_force_torque.cpp
+src/sensor/sensor_inertial_kinematics.cpp
+src/sensor/sensor_point_feet_nomove.cpp
   )
 
 # create the shared library
 ADD_LIBRARY(${PLUGIN_NAME}
   SHARED
   ${SRCS_CAPTURE}
-  ${SRCS_COMMON}
-  ${SRCS_DTASSC}
-  ${SRCS_FACTOR}
   ${SRCS_FEATURE}
-  ${SRCS_FRAME}
-  ${SRCS_HARDWARE}
-  ${SRCS_LANDMARK}
-  ${SRCS_MAP}
-  ${SRCS_MATH}
-  ${SRCS_PROBLEM}
   ${SRCS_PROCESSOR}
   ${SRCS_SENSOR}
-  ${SRCS_SOLVER}
-  ${SRCS_STATE_BLOCK}
-  ${SRCS_TRAJECTORY}
-  ${SRCS_UTILS}
-  ${SRCS_WRAPPER}
-  ${SRCS_YAML}
   )
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolf_LIBRARIES})
-# Link the library with the required dependencies
-# TARGET_LINK_LIBRARIES(${PLUGIN_NAME} )
 
+# Set compiler options
+# ====================
+if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+  message(STATUS "Using C++ compiler clang")
+  target_compile_options(${PLUGIN_NAME} PRIVATE -Winconsistent-missing-override)
+  # using Clang
+elseif (CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+  message(STATUS "Using C++ compiler gnu")
+  target_compile_options(${PLUGIN_NAME} PRIVATE -Wsuggest-override)
+  # using GCC
+endif()
+
+#Link the created libraries
+#===============EXAMPLE=========================
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfimu_LIBRARIES})
+
+# Build demos
 IF(BUILD_DEMOS)
   #Build examples
   MESSAGE("Building demos.")
@@ -237,68 +194,36 @@ IF(BUILD_DEMOS)
 ENDIF(BUILD_DEMOS)
 
 #Build tests
-#===============EXAMPLE=========================
 IF(BUILD_TESTS)
   MESSAGE("Building tests.")
   add_subdirectory(test)
 ENDIF(BUILD_TESTS)
 
 #install library
-
 #=============================================================
 INSTALL(TARGETS ${PLUGIN_NAME} EXPORT ${PLUGIN_NAME}Targets
   RUNTIME DESTINATION bin
-  LIBRARY DESTINATION lib/iri-algorithms
-  ARCHIVE DESTINATION lib/iri-algorithms)
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib)
 
 install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/cmake/${PLUGIN_NAME})
 #install headers
-INSTALL(FILES ${HDRS_MATH}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/math)
-INSTALL(FILES ${HDRS_UTILS}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/utils)
-INSTALL(FILES ${HDRS_PROBLEM}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/problem)
-INSTALL(FILES ${HDRS_HARDWARE}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/hardware)
-INSTALL(FILES ${HDRS_TRAJECTORY}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/trajectory)
-INSTALL(FILES ${HDRS_MAP}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/map)
-INSTALL(FILES ${HDRS_FRAME}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/frame)
-INSTALL(FILES ${HDRS_STATE_BLOCK}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/state_block)
-INSTALL(FILES ${HDRS_COMMON}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/common)
-INSTALL(FILES ${HDRS_DTASSC}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/association)
 INSTALL(FILES ${HDRS_CAPTURE}
    DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/capture)
 INSTALL(FILES ${HDRS_FACTOR}
   DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/factor)
 INSTALL(FILES ${HDRS_FEATURE}
   DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/feature)
-INSTALL(FILES ${HDRS_SENSOR}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/sensor)
+INSTALL(FILES ${HDRS_MATH}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/math)
 INSTALL(FILES ${HDRS_PROCESSOR}
   DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/processor)
-INSTALL(FILES ${HDRS_LANDMARK}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/landmark)
-INSTALL(FILES ${HDRS_WRAPPER}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/ceres_wrapper)
-INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/solver_suitesparse)
-INSTALL(FILES ${HDRS_SOLVER}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/solver)
-INSTALL(FILES ${HDRS_YAML}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/yaml)
+INSTALL(FILES ${HDRS_SENSOR}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/sensor)
 
 FILE(WRITE ${PROJECT_NAME}.found "")
 INSTALL(FILES ${PROJECT_NAME}.found
   DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME})
-
-
 INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
 DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/internal)
 
@@ -306,7 +231,6 @@ INSTALL(FILES "${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake" DES
 
 export(PACKAGE ${PLUGIN_NAME})
 
-#-END_SRC --------------------------------------------------------------------------------------------------------------------------------
 FIND_PACKAGE(Doxygen)
 
 FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)

README.md

 WOLF - Windowed Localization Frames | bodydynamics Plugin
 ===================================
 
-For installation guide and code documentation, please visit the [documentation website](http://mobile_robotics.pages.iri.upc-csic.es/wolf_projects/wolf_lib/wolf_doc/).
+For installation guide and code documentation, please visit the [documentation website](http://mobile_robotics.pages.iri.upc-csic.es/wolf_projects/wolf_lib/wolf-doc-sphinx/).
 
-TODO
+TODO
\ No newline at end of file

cmake_modules/wolfConfig.cmake

-#edit the following line to add the librarie's header files
-FIND_PATH(
-    wolfbodydynamics_INCLUDE_DIRS
-    NAMES bodydynamics.found
-    PATHS /usr/local/include/iri-algorithms/wolf/plugin_bodydynamics)
-IF(wolfbodydynamics_INCLUDE_DIRS)
-  MESSAGE("Found bodydynamics include dirs: ${wolfbodydynamics_INCLUDE_DIRS}")
-ELSE(wolfbodydynamics_INCLUDE_DIRS)
-  MESSAGE("Couldn't find bodydynamics include dirs")
-ENDIF(wolfbodydynamics_INCLUDE_DIRS)
-
-FIND_LIBRARY(
-    wolfbodydynamics_LIBRARIES
-    NAMES libwolfbodydynamics.so libwolfbodydynamics.dylib
-    PATHS /usr/local/lib/iri-algorithms)
-IF(wolfbodydynamics_LIBRARIES)
-  MESSAGE("Found bodydynamics lib: ${wolfbodydynamics_LIBRARIES}")
-ELSE(wolfbodydynamics_LIBRARIES)
-  MESSAGE("Couldn't find wolf bodydynamics lib")
-ENDIF(wolfbodydynamics_LIBRARIES)
-
-IF (wolfbodydynamics_INCLUDE_DIRS AND wolfbodydynamics_LIBRARIES)
-   SET(wolfbodydynamics_FOUND TRUE)
- ELSE(wolfbodydynamics_INCLUDE_DIRS AND wolfbodydynamics_LIBRARIES)
-   set(wolfbodydynamics_FOUND FALSE)
-ENDIF (wolfbodydynamics_INCLUDE_DIRS AND wolfbodydynamics_LIBRARIES)
-
-IF (wolfbodydynamics_FOUND)
-   IF (NOT wolfbodydynamics_FIND_QUIETLY)
-      MESSAGE(STATUS "Found bodydynamics: ${wolfbodydynamics_LIBRARIES}")
-   ENDIF (NOT wolfbodydynamics_FIND_QUIETLY)
-ELSE (wolfbodydynamics_FOUND)
-   IF (wolfbodydynamics_FIND_REQUIRED)
-      MESSAGE(FATAL_ERROR "Could not find wolf bodydynamics")
-   ENDIF (wolfbodydynamics_FIND_REQUIRED)
-ENDIF (wolfbodydynamics_FOUND)
-
-
-macro(wolf_report_not_found REASON_MSG)
-  set(wolfbodydynamics_FOUND FALSE)
-  unset(wolfbodydynamics_INCLUDE_DIRS)
-  unset(wolfbodydynamics_LIBRARIES)
-
-  # Reset the CMake module path to its state when this script was called.
-  set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
-
-  # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
-  # FindPackage() use the camelcase library name, not uppercase.
-  if (wolfbodydynamics_FIND_QUIETLY)
-    message(STATUS "Failed to find wolfbodydynamics- " ${REASON_MSG} ${ARGN})
-  else (wolfbodydynamics_FIND_REQUIRED)
-    message(FATAL_ERROR "Failed to find wolfbodydynamics - " ${REASON_MSG} ${ARGN})
-  else()
-    # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
-    # that prevents generation, but continues configuration.
-    message(SEND_ERROR "Failed to find wolfbodydynamics - " ${REASON_MSG} ${ARGN})
-  endif ()
-  return()
-endmacro(wolf_report_not_found)
-
-if(NOT wolfbodydynamics_FOUND)
-  wolf_report_not_found("Something went wrong while setting up wolf bodydynamics.")
-endif(NOT wolfbodydynamics_FOUND)
-# Set the include directories for wolf (itself).
-set(wolfbodydynamics_FOUND TRUE)
-
-# Suppose that our plugin requires openCV & vision_utils
-
-FIND_PACKAGE(vision_utils REQUIRED)
-list(APPEND wolfbodydynamics_INCLUDE_DIRS ${vision_utils_INCLUDE_DIR})
-list(APPEND wolfbodydynamics_LIBRARIES ${vision_utils_LIBRARY})
-
-FIND_PACKAGE(OpenCV REQUIRED)
-list(APPEND wolfbodydynamics_INCLUDE_DIRS ${OpenCV_INCLUDE_DIRS})
-list(APPEND wolfbodydynamics_LIBRARIES ${OpenCV_LIBS})
-
-#Making sure wolf is looked for
-if(NOT wolf_FOUND)
-  FIND_PACKAGE(wolf REQUIRED)
-
-  #We reverse in order to insert at the start
-  list(REVERSE wolfbodydynamics_INCLUDE_DIRS)
-  list(APPEND wolfbodydynamics_INCLUDE_DIRS ${wolf_INCLUDE_DIRS})
-  list(REVERSE wolfbodydynamics_INCLUDE_DIRS)
-
-  list(REVERSE wolfbodydynamics_LIBRARIES)
-  list(APPEND wolfbodydynamics_LIBRARIES ${wolf_LIBRARIES})
-  list(REVERSE wolfbodydynamics_LIBRARIES)
-endif()
\ No newline at end of file

cmake_modules/wolfbodydynamicsConfig.cmake

@@ -12,7 +12,7 @@ ENDIF(wolfbodydynamics_INCLUDE_DIRS)
 FIND_LIBRARY(
     wolfbodydynamics_LIBRARIES
     NAMES libwolfbodydynamics.so
-    PATHS /usr/local/lib/iri-algorithms)
+    PATHS /usr/local/lib)
 IF(wolfbodydynamics_LIBRARIES)
   MESSAGE("Found wolf bodydynamics lib: ${wolfbodydynamics_LIBRARIES}")
 ELSE(wolfbodydynamics_LIBRARIES)
@@ -73,14 +73,19 @@ set(wolfbodydynamics_FOUND TRUE)
 
 # Making sure that wolf is always looked for
 if(NOT wolf_FOUND)
-  FIND_PACKAGE(wolf REQUIRED)
+  FIND_PACKAGE(wolfcore REQUIRED)
 
   #We reverse in order to insert at the start
   list(REVERSE wolfbodydynamics_INCLUDE_DIRS)
-  list(APPEND wolfbodydynamics_INCLUDE_DIRS ${wolf_INCLUDE_DIRS})
+  list(APPEND wolfbodydynamics_INCLUDE_DIRS ${wolfcore_INCLUDE_DIRS})
   list(REVERSE wolfbodydynamics_INCLUDE_DIRS)
 
   list(REVERSE wolfbodydynamics_LIBRARIES)
-  list(APPEND wolfbodydynamics_LIBRARIES ${wolf_LIBRARIES})
+  list(APPEND wolfbodydynamics_LIBRARIES ${wolfcore_LIBRARIES})
   list(REVERSE wolfbodydynamics_LIBRARIES)
-endif()
+endif()
\ No newline at end of file
+
+# provide both INCLUDE_DIR and INCLUDE_DIRS
+SET(wolfbodydynamics_INCLUDE_DIR ${wolfbodydynamics_INCLUDE_DIRS})
+# provide both LIBRARY and LIBRARIES 
+SET(wolfbodydynamics_LIBRARY ${wolfbodydynamics_LIBRARIES})
\ No newline at end of file

demos/CMakeLists.txt

+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fext-numeric-literals")  # necessary for files using boost 
+
+FIND_PACKAGE(pinocchio REQUIRED)
+FIND_PACKAGE(multicontact-api REQUIRED)
+
+# SYSTEM disables warnings from library headers
+include_directories(
+    SYSTEM ${PINOCCHIO_INCLUDE_DIRS}
+)
+
+add_library(mcapi_utils mcapi_utils.cpp)
+
+# add_executable(mcapi_povcdl_estimation mcapi_povcdl_estimation.cpp)
+# target_link_libraries(mcapi_povcdl_estimation
+#     mcapi_utils
+#     ${wolfcore_LIBRARIES}
+#     ${wolfimu_LIBRARIES}
+#     ${PLUGIN_NAME}
+#     ${multicontact-api_LIBRARIES}
+#     ${pinocchio_LIBRARIES}
+# )
+# target_compile_definitions(mcapi_povcdl_estimation PRIVATE ${PINOCCHIO_CFLAGS_OTHER})
+
+
+# add_executable(mcapi_pov_estimation mcapi_pov_estimation.cpp)
+# target_link_libraries(mcapi_pov_estimation
+#     mcapi_utils
+#     ${wolfcore_LIBRARIES}
+#     ${wolfimu_LIBRARIES}
+#     ${PLUGIN_NAME}
+#     ${multicontact-api_LIBRARIES}
+#     ${pinocchio_LIBRARIES}
+# )
+# target_compile_definitions(mcapi_pov_estimation PRIVATE ${PINOCCHIO_CFLAGS_OTHER})
+
+add_executable(solo_real_povcdl_estimation solo_real_povcdl_estimation.cpp)
+target_link_libraries(solo_real_povcdl_estimation 
+    mcapi_utils
+    ${wolfcore_LIBRARIES}
+    ${wolfimu_LIBRARIES}
+    ${PLUGIN_NAME}
+    ${pinocchio_LIBRARIES}    
+    /usr/local/lib/libcnpy.so
+    z
+)
+
+add_executable(solo_real_pov_estimation solo_real_pov_estimation.cpp)
+target_link_libraries(solo_real_pov_estimation 
+    mcapi_utils
+    ${wolfcore_LIBRARIES}
+    ${wolfimu_LIBRARIES}
+    ${PLUGIN_NAME}
+    ${pinocchio_LIBRARIES}    
+    /usr/local/lib/libcnpy.so
+    z
+)
+
+add_executable(solo_mocap_imu solo_mocap_imu.cpp)
+target_link_libraries(solo_mocap_imu 
+    ${wolfcore_LIBRARIES}
+    ${wolfimu_LIBRARIES}
+    ${PLUGIN_NAME}
+    ${pinocchio_LIBRARIES}    
+    /usr/local/lib/libcnpy.so
+    z
+)
+
+# add_executable(test_cnpy test_cnpy.cpp)
+# target_link_libraries(test_cnpy
+#     /usr/local/lib/libcnpy.so
+# )
+

demos/eigenmvn.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/**
+ * Multivariate Normal distribution sampling using C++11 and Eigen matrices.
+ * 
+ * This is taken from http://stackoverflow.com/questions/16361226/error-while-creating-object-from-templated-class
+ * (also see http://lost-found-wandering.blogspot.fr/2011/05/sampling-from-multivariate-normal-in-c.html)
+ * 
+ * I have been unable to contact the original author, and I've performed
+ * the following modifications to the original code:
+ * - removal of the dependency to Boost, in favor of straight C++11;
+ * - ability to choose from Solver or Cholesky decomposition (supposedly faster);
+ * - fixed Cholesky by using LLT decomposition instead of LDLT that was not yielding
+ *   a correctly rotated variance 
+ *   (see this http://stats.stackexchange.com/questions/48749/how-to-sample-from-a-multivariate-normal-given-the-pt-ldlt-p-decomposition-o )
+ */
+
+/**
+ * Copyright (c) 2014 by Emmanuel Benazera beniz@droidnik.fr, All rights reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3.0 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+ * 
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library.
+ */
+
+#ifndef __EIGENMULTIVARIATENORMAL_HPP
+#define __EIGENMULTIVARIATENORMAL_HPP
+
+#include <Eigen/Dense>
+#include <random>
+
+/*
+  We need a functor that can pretend it's const,
+  but to be a good random number generator 
+  it needs mutable state.  The standard Eigen function 
+  Random() just calls rand(), which changes a global
+  variable.
+*/
+namespace Eigen {
+  namespace internal {
+    template<typename Scalar>
+      struct scalar_normal_dist_op
+      {
+	static std::mt19937 rng;                        // The uniform pseudo-random algorithm
+	mutable std::normal_distribution<Scalar> norm; // gaussian combinator
+	
+	EIGEN_EMPTY_STRUCT_CTOR(scalar_normal_dist_op)
+
+	template<typename Index>
+	inline const Scalar operator() (Index, Index = 0) const { return norm(rng); }
+	inline void seed(const uint64_t &s) { rng.seed(s); }
+      };
+
+    template<typename Scalar>
+      std::mt19937 scalar_normal_dist_op<Scalar>::rng;
+      
+    template<typename Scalar>
+      struct functor_traits<scalar_normal_dist_op<Scalar> >
+      { enum { Cost = 50 * NumTraits<Scalar>::MulCost, PacketAccess = false, IsRepeatable = false }; };
+
+  } // end namespace internal
+
+  /**
+    Find the eigen-decomposition of the covariance matrix
+    and then store it for sampling from a multi-variate normal 
+  */
+  template<typename Scalar>
+    class EigenMultivariateNormal
+  {
+    Matrix<Scalar,Dynamic,Dynamic> _covar;
+    Matrix<Scalar,Dynamic,Dynamic> _transform;
+    Matrix< Scalar, Dynamic, 1> _mean;
+    internal::scalar_normal_dist_op<Scalar> randN; // Gaussian functor
+    bool _use_cholesky;
+    SelfAdjointEigenSolver<Matrix<Scalar,Dynamic,Dynamic> > _eigenSolver; // drawback: this creates a useless eigenSolver when using Cholesky decomposition, but it yields access to eigenvalues and vectors
+    
+  public:
+  EigenMultivariateNormal(const Matrix<Scalar,Dynamic,1>& mean,const Matrix<Scalar,Dynamic,Dynamic>& covar,
+			  const bool use_cholesky=false,const uint64_t &seed=std::mt19937::default_seed)
+      :_use_cholesky(use_cholesky)
+     {
+        randN.seed(seed);
+	setMean(mean);
+	setCovar(covar);
+      }
+
+    void setMean(const Matrix<Scalar,Dynamic,1>& mean) { _mean = mean; }
+    void setCovar(const Matrix<Scalar,Dynamic,Dynamic>& covar)
+    {
+      _covar = covar;
+      
+      // Assuming that we'll be using this repeatedly,
+      // compute the transformation matrix that will
+      // be applied to unit-variance independent normals
+      
+      if (_use_cholesky)
+	{
+	  Eigen::LLT<Eigen::Matrix<Scalar,Dynamic,Dynamic> > cholSolver(_covar);
+	  // We can only use the cholesky decomposition if 
+	  // the covariance matrix is symmetric, pos-definite.
+	  // But a covariance matrix might be pos-semi-definite.
+	  // In that case, we'll go to an EigenSolver
+	  if (cholSolver.info()==Eigen::Success)
+	    {
+	      // Use cholesky solver
+	      _transform = cholSolver.matrixL();
+	    }
+	  else
+	    {
+	      throw std::runtime_error("Failed computing the Cholesky decomposition. Use solver instead");
+	    }
+	}
+      else
+	{
+	  _eigenSolver = SelfAdjointEigenSolver<Matrix<Scalar,Dynamic,Dynamic> >(_covar);
+	  _transform = _eigenSolver.eigenvectors()*_eigenSolver.eigenvalues().cwiseMax(0).cwiseSqrt().asDiagonal();
+	}
+    }
+
+    /// Draw nn samples from the gaussian and return them
+    /// as columns in a Dynamic by nn matrix
+    Matrix<Scalar,Dynamic,-1> samples(int nn)
+      {
+	return (_transform * Matrix<Scalar,Dynamic,-1>::NullaryExpr(_covar.rows(),nn,randN)).colwise() + _mean;
+      }
+  }; // end class EigenMultivariateNormal
+} // end namespace Eigen
+#endif

demos/mcapi_povcdl_estimation.yaml

+# trajectory handling
+dt: 0.001
+min_t: -1.0  # -1 means from the beginning of the trajectory
+max_t: -1  # -1 means until the end of the trajectory
+solve_every_sec: -1   # < 0 strict --> no solve
+solve_end: False
+# solve_every_sec: 0.3   # < 0 strict --> no solve
+# solve_end: True
+
+# estimator sensor noises
+std_acc_est: 0.001
+std_gyr_est: 0.001
+std_pbc_est: 0.001
+std_vbc_est: 0.001   # higher than simulated -> has to take care of the non-modeled bias as well... Good value for solo sin traj
+std_f_est:   0.001
+std_tau_est: 0.001
+# std_odom3d_est:  0.000001
+std_odom3d_est:  100000
+
+# simulated sensor noises
+std_acc_sim: 0.0001
+std_gyr_sim: 0.0001
+std_pbc_sim: 0.0001
+std_vbc_sim: 0.0001
+std_f_sim:   0.0001
+std_tau_sim: 0.0001
+std_odom3d_sim:  100000000
+
+# some controls
+fz_thresh: 1
+noisy_measurements: False
+
+# priors
+std_prior_p: 0.01
+std_prior_o: 0.1
+std_prior_v: 0.1
+
+# std_abs_biasimu: 100000
+std_abs_biasimu: 0.0000001  # almost fixed
+std_abs_bias_pbc: 10000   # noprior  
+# std_abs_bias_pbc: 0.00001  # almost fixed 
+std_bp_drift: 10000     # All the drift you want
+# std_bp_drift: 0.1     # no drift
+
+bias_pbc_prior: [0,0,0]
+
+
+# model disturbance
+scale_dist: 0.00  # disturbance of link inertia
+mass_dist: False
+base_dist_only: False
+
+# which measurement/parameter is disturbed?
+vbc_is_dist:   False
+Iw_is_dist:    False
+Lgest_is_dist: False
+
+
+# Robot model
+# robot_urdf: "/opt/openrobots/share/example-robot-data/robots/talos_data/robots/talos_reduced.urdf"
+# dimc: 6
+robot_urdf: "/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo12.urdf"
+dimc: 3
+
+
+# Trajectory files
+# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/talos_sin_traj.cs"
+# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/talos_contact_switch.cs"
+# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_traj.cs"
+contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_traj_pyb.cs"
+
+# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_y_notrunk.cs"
+# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_r+y.cs"
+
+# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_r+z.cs"
+# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_stamping.cs"

demos/mcapi_utils.cpp

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+
+#include "mcapi_utils.h"
+
+Vector3d computeAccFromSpatial(const Vector3d& ddr, const Vector3d& w, const Vector3d& dr)
+{
+    return ddr + w.cross(dr);
+}
+
+
+std::vector<Vector3d> contacts_from_footrect_center()
+{
+    // compute feet corners coordinates like from the leg_X_6_joint
+    double lx = 0.1;
+    double ly = 0.065;
+    double lz = 0.107;
+    std::vector<Vector3d> contacts; contacts.resize(4);
+    contacts[0] = {-lx, -ly, -lz};
+    contacts[1] = {-lx,  ly, -lz};
+    contacts[2] = { lx, -ly, -lz};
+    contacts[3] = { lx,  ly, -lz};
+    return contacts;
+}
+
+
+Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contacts, 
+                                 const Matrix<double, 12, 1>& cf12)
+{
+    Vector3d f = cf12.segment<3>(0) + cf12.segment<3>(3) + cf12.segment<3>(6) + cf12.segment<3>(9);
+    Vector3d tau = contacts[0].cross(cf12.segment<3>(0))
+                 + contacts[1].cross(cf12.segment<3>(3))
+                 + contacts[2].cross(cf12.segment<3>(6))
+                 + contacts[3].cross(cf12.segment<3>(9));
+    Matrix<double, 6, 1> wrench; wrench << f, tau;
+    return wrench;
+}
+
+
+Matrix3d compute_Iw(pinocchio::Model& model, 
+                    pinocchio::Data& data, 
+                    VectorXd& q_static, 
+                    Vector3d& b_p_bc)
+{
+    MatrixXd b_Mc = pinocchio::crba(model, data, q_static);  // mass matrix at b frame expressed in b frame
+    // MatrixXd b_Mc = crba(model_dist, data_dist, q_static);  // mass matrix at b frame expressed in b frame
+    MatrixXd b_I_b = b_Mc.topLeftCorner<6,6>();             // inertia matrix at b frame expressed in b frame 
+    pinocchio::SE3 bTc (Eigen::Matrix3d::Identity(), b_p_bc);          // "no free flyer robot -> c frame oriented the same as b"
+    pinocchio::SE3 cTb = bTc.inverse();        
+    // Ic = cXb^star * Ib * bXc = bXc^T * Ib * bXc 
+    // c_I_c block diagonal:
+    // [m*Id3, 03;
+    //  0,     Iw]
+    MatrixXd c_I_c = cTb.toDualActionMatrix() * b_I_b * bTc.toActionMatrix();
+    Matrix3d b_Iw  = c_I_c.bottomRightCorner<3,3>();  // meas
+    return b_Iw;
+}

demos/mcapi_utils.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include <vector>
+#include "Eigen/Dense"
+#include "pinocchio/algorithm/crba.hpp"
+
+
+using namespace Eigen; 
+
+
+/**
+ * \brief Compute a 3D acceleration from 6D spatial acceleration
+ * 
+ * \param ddr spatial acc linear part
+ * \param w spatial acc rotational part
+ * \param dr spatial velocity linear part
+**/
+Vector3d computeAccFromSpatial(const Vector3d& ddr, const Vector3d& w, const Vector3d& dr);
+
+
+/**
+* \brief Compute the relative contact points from foot center of a Talos foot.
+* 
+* Order is clockwise, starting from bottom left (looking at a forward pointing foot from above).
+* Expressed in local foot coordinates. 
+**/
+std::vector<Vector3d> contacts_from_footrect_center();
+
+
+/**
+* \brief Compute the wrench at the end effector center expressed in world coordinates.
+
+* Each contact force (at a foot for instance) is represented in MAPI as a set of 
+* 4 forces at the corners of the contact polygone (foot -> rectangle). These forces,
+* as the other quantities, are expressed in world coordinates. From this 4 3D forces, 
+* we can compute the 6D wrench at the center of the origin point of the contacts vectors.
+
+* \param contacts std vector of 3D contact forces
+* \param cforces 12D corner forces vector ordered as [fx1, fy1, fz1, fx2, fy2... fz4], same order as contacts
+* \param wRl rotation matrix, orientation from world frame to foot frame
+**/
+Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contacts, 
+                                             const Matrix<double, 12, 1>& cf12);
+
+
+/**
+* \brief Compute the centroidal angular inertia used to compute the angular momentum.
+
+* \param model: pinocchio robot model used
+* \param data: pinocchio robot data used
+* \param q_static: configuration of the robot with free flyer pose at origin (local base frame = world frame)
+* \param b_p_bc: measure of the CoM position relative to the base
+**/
+Matrix3d compute_Iw(pinocchio::Model& model, 
+                    pinocchio::Data& data, 
+                    VectorXd& q_static, 
+                    Vector3d& b_p_bc);

demos/processor_ft_preint.yaml

+type: "ProcessorForceTorquePreint"         # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+name: "PFTPreint"    # This is ignored. The name provided to the SensorFactory prevails
+time_tolerance: 0.0005         # Time tolerance for joining KFs
+unmeasured_perturbation_std: 0.0000000
+sensor_ikin_name: "SenIK"
+sensor_angvel_name: "SenImu"
+keyframe_vote:
+    max_time_span:      100000000000   # seconds
+    max_buff_length:    50000   # motion deltas
+    dist_traveled:      20000.0   # meters
+    angle_turned:       1000   # radians (1 rad approx 57 deg, approx 60 deg)
+    voting_active:      false
\ No newline at end of file

demos/processor_imu_mcapi_demo.yaml

+type: "ProcessorImu"         # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+name: "Main imu"             # This is ignored. The name provided to the SensorFactory prevails
+time_tolerance: 0.0005         # Time tolerance for joining KFs
+unmeasured_perturbation_std: 0.000001
+keyframe_vote:
+    max_time_span:      0.29999  # seconds
+    max_buff_length:    100000000000   # motion deltas
+    dist_traveled:      20000.0   # meters
+    angle_turned:       1000   # radians (1 rad approx 57 deg, approx 60 deg)
+    voting_active:      true
+    voting_aux_active:  false
\ No newline at end of file

demos/processor_imu_solo12.yaml

+keyframe_vote:
+  angle_turned: 1000
+  dist_traveled: 20000.0
+  max_buff_length: 100000000000
+  max_time_span: 0.19999
+  voting_active: true
+name: Main imu
+time_tolerance: 0.0005
+type: ProcessorImu
+unmeasured_perturbation_std: 1.0e-06

demos/processor_inertial_kinematics.yaml

+type: "ProcessorInertialKinematics"             # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+name: "PInertialKinematics"        # This is ignored. The name provided to the SensorFactory prevails
+
+sensor_angvel_name: "SenImu" 
+std_bp_drift: 0.01
\ No newline at end of file

demos/processor_odom_3d.yaml

+type: "ProcessorOdom3d"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+time_tolerance: 0.0005  # seconds
+unmeasured_perturbation_std: 0.0001
+
+keyframe_vote:
+  voting_active:        false
+  max_time_span:          100   # seconds
+  max_buff_length:        10    # motion deltas
+  dist_traveled:          0.5   # meters
+  angle_turned:           0.1   # radians (1 rad approx 57 deg, approx 60 deg)

demos/sensor_ft.yaml

+type: "SensorForceTorque"             # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+name: "SenFT"        # This is ignored. The name provided to the SensorFactory prevails
+mass: 10
+std_f: 0.001 
+std_tau: 0.001
\ No newline at end of file

demos/sensor_imu.yaml

-type: "SensorIMU"             # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-name: "Main IMU Sensor"        # This is ignored. The name provided to the SensorFactory prevails
+type: "SensorImu"             # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+name: "Main Imu Sensor"        # This is ignored. The name provided to the SensorFactory prevails
 motion_variances: 
    a_noise:                0.02     # standard deviation of Acceleration noise (same for all the axis) in m/s2
    w_noise:                0.03    # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
    ab_initial_stdev:       0.00     # m/s2    - initial bias 
    wb_initial_stdev:       0.0     # rad/sec - initial bias 
-   ab_rate_stdev:          0.0001       # m/s2/sqrt(s)           
-   wb_rate_stdev:          0.0001    # rad/s/sqrt(s)
+   ab_rate_stdev:          0.00001       # m/s2/sqrt(s)           
+   wb_rate_stdev:          0.00001    # rad/s/sqrt(s)
\ No newline at end of file