From ed28d72e436bd2aefcc6b465894a0abafe24f417 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 30 Jul 2024 10:52:54 +0200 Subject: [PATCH 01/11] Updated gitignore to ignore vs files --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.gitignore b/.gitignore index 51151a4e..2fcbd748 100644 --- a/.gitignore +++ b/.gitignore @@ -46,3 +46,6 @@ compile_commands.json CMakeLists.txt.user .vscode/ +# Visual studio +.vs/* +CMakeSettings.json From 34039a4ca28ea79db4e28cce3e9de32a951f4e42 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 30 Jul 2024 10:52:34 +0200 Subject: [PATCH 02/11] Use conda in CI instead of vcpkg and source installation --- .github/workflows/ci.yml | 193 +++++++++------------------------------ 1 file changed, 45 insertions(+), 148 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 1dafbd63..4dbda67a 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -17,159 +17,61 @@ jobs: matrix: build_type: [Release] os: [ubuntu-latest, windows-latest, macOS-latest] + fail-fast: false + steps: - uses: actions/checkout@master - # Print environment variables to simplify development and debugging - - name: Environment Variables - shell: bash - run: env + - uses: conda-incubator/setup-miniconda@v2 + with: + miniforge-variant: Miniforge3 + miniforge-version: latest + channels: conda-forge,robostack-staging,robotology + channel-priority: true # ============ # DEPENDENCIES # ============ - # Remove apt repos that are known to break from time to time - # See https://github.com/actions/virtual-environments/issues/323 - - name: Remove broken apt repos [Ubuntu] - if: matrix.os == 'ubuntu-latest' + - name: Dependencies + shell: bash -l {0} run: | - for apt_file in `grep -lr microsoft /etc/apt/sources.list.d/`; do sudo rm $apt_file; done - - - name: Dependencies [Windows] - if: matrix.os == 'windows-latest' + # Workaround for https://github.com/conda-incubator/setup-miniconda/issues/186 + conda config --remove channels defaults + # Compilation related dependencies + conda install cmake compilers make ninja pkg-config + # Actual dependencies + conda install yarp ycm-cmake-modules icub-main eigen idyntree bipedal-locomotion-framework human-dynamics-estimation wearables + + - name: Linux-only Dependencies [Linux] + if: contains(matrix.os, 'ubuntu') + shell: bash -l {0} run: | - git clone https://github.com/robotology-dependencies/robotology-vcpkg-binary-ports C:/robotology-vcpkg-binary-ports - vcpkg.exe --overlay-ports=C:/robotology-vcpkg-binary-ports install --triplet x64-windows ace libxml2 eigen3 ipopt-binary catch2 + conda install mesa-libgl-devel-cos7-x86_64 - - name: Dependencies [macOS] - if: matrix.os == 'macOS-latest' + - name: Windows-only Dependencies [Windows] + if: contains(matrix.os, 'windows') + shell: bash -l {0} run: | - brew install ace boost eigen swig qt5 orocos-kdl catch2 + conda install vs2019_win-64 - - name: Dependencies [Ubuntu] - if: matrix.os == 'ubuntu-latest' + - name: Windows-workarounds [Windows] + if: contains(matrix.os, 'windows') + shell: cmd /C CALL {0} run: | - sudo apt-get update - sudo apt-get install git build-essential cmake libace-dev coinor-libipopt-dev libboost-system-dev libboost-filesystem-dev \ - libboost-thread-dev liborocos-kdl-dev libeigen3-dev swig qtbase5-dev qtdeclarative5-dev qtmultimedia5-dev \ - libxml2-dev liburdfdom-dev libtinyxml-dev liburdfdom-dev liboctave-dev python3-dev valgrind - - - name: Source-based Dependencies [Windows] - if: matrix.os == 'windows-latest' - shell: bash + :: Due to this https://github.com/conda-forge/icub-models-feedstock/issues/18 + :: pcl is removed as a workaround for https://github.com/ami-iit/bipedal-locomotion-framework/pull/695#issuecomment-1632208836 + :: pcl can be re-added once we have a ros humble build compatible with PCL 1.13.0 + :: pybind11 constrained as workaround for https://github.com/conda-forge/pybind11-feedstock/issues/95 + conda install "pybind11<2.12.0" + conda remove icub-models pcl + + - name: Print used environment + shell: bash -l {0} run: | - # YCM - cd ${GITHUB_WORKSPACE} - git clone https://github.com/robotology/ycm - cd ycm - mkdir -p build - cd build - cmake -A x64 -DCMAKE_TOOLCHAIN_FILE=${VCPKG_INSTALLATION_ROOT}/scripts/buildsystems/vcpkg.cmake \ - -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install .. - - cmake --build . --config ${{ matrix.build_type }} --target INSTALL - - - # YARP - cd ${GITHUB_WORKSPACE} - git clone https://github.com/robotology/yarp - cd yarp - mkdir -p build - cd build - cmake -A x64 -DCMAKE_TOOLCHAIN_FILE=${VCPKG_INSTALLATION_ROOT}/scripts/buildsystems/vcpkg.cmake \ - -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install .. - cmake --build . --config ${{ matrix.build_type }} --target INSTALL - # Workaround for https://github.com/robotology-dependencies/robotology-vcpkg-binary-ports/issues/3 - export IPOPT_DIR=${VCPKG_INSTALLATION_ROOT}/installed/x64-windows - - # iDynTree - cd ${GITHUB_WORKSPACE} - git clone https://github.com/robotology/iDynTree - cd iDynTree - mkdir -p build - cd build - cmake -A x64 -DCMAKE_TOOLCHAIN_FILE=${VCPKG_INSTALLATION_ROOT}/scripts/buildsystems/vcpkg.cmake \ - -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install \ - -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \ - -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install .. - cmake --build . --config ${{ matrix.build_type }} --target install - - # icub-main - cd ${GITHUB_WORKSPACE} - git clone https://github.com/robotology/icub-main.git - cd icub-main && mkdir -p build && cd build - cmake -A x64 -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \ - -DENABLE_icubmod_cartesiancontrollerserver=ON -DENABLE_icubmod_cartesiancontrollerclient=ON -DENABLE_icubmod_gazecontrollerclient=ON .. - cmake --build . --config ${{ matrix.build_type }} --target install - - # wearables - cd ${GITHUB_WORKSPACE} - git clone https://github.com/robotology/wearables.git - cd wearables - mkdir -p build - cd build - cmake -A x64 -DCMAKE_TOOLCHAIN_FILE=${VCPKG_INSTALLATION_ROOT}/scripts/buildsystems/vcpkg.cmake \ - -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install \ - -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \ - -DXSENS_MVN_USE_SDK:BOOL=OFF \ - -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install .. - cmake --build . --config ${{ matrix.build_type }} --target install - - - - name: Source-based Dependencies [Ubuntu/macOS] - if: matrix.os == 'ubuntu-latest' || matrix.os == 'macOS-latest' - shell: bash - run: | - # YCM - cd ${GITHUB_WORKSPACE} - git clone https://github.com/robotology/ycm - cd ycm - mkdir -p build - cd build - cmake -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install .. - cmake --build . --config ${{ matrix.build_type }} --target install - - # YARP - cd ${GITHUB_WORKSPACE} - git clone https://github.com/robotology/yarp - cd yarp - mkdir -p build - cd build - cmake -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install .. - cmake --build . --config ${{ matrix.build_type }} --target install - - # iDynTree - cd ${GITHUB_WORKSPACE} - git clone https://github.com/robotology/iDynTree - cd iDynTree - mkdir -p build - cd build - cmake -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \ - -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install .. - cmake --build . --config ${{ matrix.build_type }} --target install - - # icub-main - cd ${GITHUB_WORKSPACE} - git clone https://github.com/robotology/icub-main.git - cd icub-main && mkdir -p build && cd build - cmake -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \ - -DENABLE_icubmod_cartesiancontrollerserver=ON -DENABLE_icubmod_cartesiancontrollerclient=ON -DENABLE_icubmod_gazecontrollerclient=ON .. - cmake --build . --config ${{ matrix.build_type }} --target install - - # wearables - cd ${GITHUB_WORKSPACE} - git clone https://github.com/robotology/wearables.git - cd wearables - mkdir -p build - cd build - cmake -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install \ - -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \ - -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install .. - cmake --build . --config ${{ matrix.build_type }} --target install - - + conda list + env # =================== # CMAKE-BASED PROJECT # =================== @@ -177,34 +79,29 @@ jobs: - name: Configure [Windows] # Use bash also on Windows (otherwise cd, mkdir, ... do not work) if: matrix.os == 'windows-latest' - shell: bash + shell: bash -l {0} run: | mkdir -p build cd build - cmake -A x64 -DCMAKE_TOOLCHAIN_FILE=${VCPKG_INSTALLATION_ROOT}/scripts/buildsystems/vcpkg.cmake \ - -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install \ + cmake -G"Visual Studio 17 2022" \ -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install .. - name: Configure [Ubuntu/macOS] if: matrix.os == 'ubuntu-latest' || matrix.os == 'macOS-latest' - shell: bash + shell: bash -l {0} run: | mkdir -p build cd build - cmake -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install \ - -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install .. + cmake -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install .. - name: Build - shell: bash + shell: bash -l {0} run: | cd build - # Attempt of fix for using YARP idl generators (that link ACE) in Windows - # See https://github.com/robotology/idyntree/issues/569 - export PATH=$PATH:${GITHUB_WORKSPACE}/install/bin:${VCPKG_ROBOTOLOGY_ROOT}/installed/x64-windows/bin cmake --build . --config ${{ matrix.build_type }} - name: Install - shell: bash + shell: bash -l {0} run: | cd build cmake --build . --config ${{ matrix.build_type }} --target install From 7ef4f251e953ddc3cb26d8b59abb696f039baab4 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 30 Jul 2024 11:42:07 +0200 Subject: [PATCH 03/11] Removed files that are included by YCM too --- cmake/AddInstallRPATHSupport.cmake | 155 ----------------------------- cmake/AddUninstallTarget.cmake | 58 ----------- 2 files changed, 213 deletions(-) delete mode 100644 cmake/AddInstallRPATHSupport.cmake delete mode 100644 cmake/AddUninstallTarget.cmake diff --git a/cmake/AddInstallRPATHSupport.cmake b/cmake/AddInstallRPATHSupport.cmake deleted file mode 100644 index 73af9348..00000000 --- a/cmake/AddInstallRPATHSupport.cmake +++ /dev/null @@ -1,155 +0,0 @@ -#.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 is aimed to solve 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 be properly -# -# 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. - -# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) -# SPDX-License-Identifier: BSD-3-Clause - -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() diff --git a/cmake/AddUninstallTarget.cmake b/cmake/AddUninstallTarget.cmake deleted file mode 100644 index bf859072..00000000 --- a/cmake/AddUninstallTarget.cmake +++ /dev/null @@ -1,58 +0,0 @@ -#.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 that will remove the files installed by your package -# (using install_manifest.txt) - -# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) -# SPDX-License-Identifier: BSD-3-Clause - - -if(DEFINED __ADD_UNINSTALL_TARGET_INCLUDED) - return() -endif() -set(__ADD_UNINSTALL_TARGET_INCLUDED TRUE) - - -set(_filename ${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake) - -file(WRITE ${_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}) - message(STATUS \"Uninstalling: \$ENV{DESTDIR}\${file}\") - if(EXISTS \"\$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 \"File \\\"\$ENV{DESTDIR}\${file}\\\" does not exist.\") - endif() -endforeach(file) -") - -if("${CMAKE_GENERATOR}" MATCHES "^(Visual Studio|Xcode)") - set(_uninstall "UNINSTALL") -else() - set(_uninstall "uninstall") -endif() -add_custom_target(${_uninstall} COMMAND "${CMAKE_COMMAND}" -P "${_filename}") -set_property(TARGET ${_uninstall} PROPERTY FOLDER "CMakePredefinedTargets") From 39ec4460dd5712d6e3fb51f0d54b1c53ea737a42 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 30 Jul 2024 11:53:37 +0200 Subject: [PATCH 04/11] Removed matlogger2 dependency and removed logging from OculusModule It is not used anymore anyhow --- app/robots/iCubErzelli02/oculusConfig.ini | 1 - app/robots/iCubGazeboV2_5/oculusConfig.ini | 1 - app/robots/iCubGenova04/oculusConfig.ini | 1 - app/robots/iCubGenova09/oculusConfig.ini | 1 - app/robots/icubGazeboSim/oculusConfig.ini | 1 - ...WalkingTeleoperationFindDependencies.cmake | 8 - modules/HapticGlove_module/CMakeLists.txt | 4 - modules/Oculus_module/CMakeLists.txt | 12 +- .../Oculus_module/include/OculusModule.hpp | 18 --- modules/Oculus_module/src/OculusModule.cpp | 146 ------------------ 10 files changed, 1 insertion(+), 192 deletions(-) diff --git a/app/robots/iCubErzelli02/oculusConfig.ini b/app/robots/iCubErzelli02/oculusConfig.ini index 11ad28c9..7e55a855 100644 --- a/app/robots/iCubErzelli02/oculusConfig.ini +++ b/app/robots/iCubErzelli02/oculusConfig.ini @@ -15,7 +15,6 @@ useXsens 0 useiFeel 1 useOpenXr 1 useSenseGlove 0 -enableLogger 0 enableMoveRobot 1 # the following value is a threshold used to update the teleoperation frame position # when the human rotates inside the virtualizer diff --git a/app/robots/iCubGazeboV2_5/oculusConfig.ini b/app/robots/iCubGazeboV2_5/oculusConfig.ini index 3fa39862..7cbbecda 100644 --- a/app/robots/iCubGazeboV2_5/oculusConfig.ini +++ b/app/robots/iCubGazeboV2_5/oculusConfig.ini @@ -14,7 +14,6 @@ robot icubSim useXsens 0 useiFeel 1 useSenseGlove 0 -enableLogger 0 enableMoveRobot 1 # the following value is a threshold used to update the teleoperation frame position # when the human rotates inside the virtualizer diff --git a/app/robots/iCubGenova04/oculusConfig.ini b/app/robots/iCubGenova04/oculusConfig.ini index 8358c1c0..ab91ca09 100644 --- a/app/robots/iCubGenova04/oculusConfig.ini +++ b/app/robots/iCubGenova04/oculusConfig.ini @@ -14,7 +14,6 @@ robot icub useXsens 0 useiFeel 1 useSenseGlove 0 -enableLogger 0 enableMoveRobot 1 # the following value is a threshold used to update the teleoperation frame position # when the human rotates inside the virtualizer diff --git a/app/robots/iCubGenova09/oculusConfig.ini b/app/robots/iCubGenova09/oculusConfig.ini index 11ad28c9..7e55a855 100644 --- a/app/robots/iCubGenova09/oculusConfig.ini +++ b/app/robots/iCubGenova09/oculusConfig.ini @@ -15,7 +15,6 @@ useXsens 0 useiFeel 1 useOpenXr 1 useSenseGlove 0 -enableLogger 0 enableMoveRobot 1 # the following value is a threshold used to update the teleoperation frame position # when the human rotates inside the virtualizer diff --git a/app/robots/icubGazeboSim/oculusConfig.ini b/app/robots/icubGazeboSim/oculusConfig.ini index b28d4d9a..79e85774 100644 --- a/app/robots/icubGazeboSim/oculusConfig.ini +++ b/app/robots/icubGazeboSim/oculusConfig.ini @@ -14,7 +14,6 @@ robot icubSim useXsens 0 useiFeel 1 useSenseGlove 0 -enableLogger 0 enableMoveRobot 1 # the following value is a threshold used to update the teleoperation frame position # when the human rotates inside the virtualizer diff --git a/cmake/WalkingTeleoperationFindDependencies.cmake b/cmake/WalkingTeleoperationFindDependencies.cmake index 125c27fb..2291c151 100644 --- a/cmake/WalkingTeleoperationFindDependencies.cmake +++ b/cmake/WalkingTeleoperationFindDependencies.cmake @@ -130,14 +130,6 @@ add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_BIND DEPENDS ENABLE_RPATH USE_LINK_PATH) -# Enable logger -option(ENABLE_LOGGER "Enable logger using matlogger2" OFF) -if(ENABLE_LOGGER) - add_definitions(-DENABLE_LOGGER) - find_package(matlogger2 REQUIRED) -endif(ENABLE_LOGGER) - - find_package(PkgConfig QUIET) if (PkgConfig_FOUND) pkg_check_modules(libfvad QUIET IMPORTED_TARGET libfvad) diff --git a/modules/HapticGlove_module/CMakeLists.txt b/modules/HapticGlove_module/CMakeLists.txt index 55f94cc4..e29ec4ce 100644 --- a/modules/HapticGlove_module/CMakeLists.txt +++ b/modules/HapticGlove_module/CMakeLists.txt @@ -70,10 +70,6 @@ set(${EXE_TARGET_NAME}_LINKED_LIBS IWear::IWear WearableActuators::WearableActuators ) -if(ENABLE_LOGGER) -list(APPEND ${EXE_TARGET_NAME}_LINKED_LIBS matlogger2::matlogger2 ) -endif() - target_link_libraries(${EXE_TARGET_NAME} LINK_PUBLIC ${${EXE_TARGET_NAME}_LINKED_LIBS} ) diff --git a/modules/Oculus_module/CMakeLists.txt b/modules/Oculus_module/CMakeLists.txt index ffb2eba3..b3aff584 100644 --- a/modules/Oculus_module/CMakeLists.txt +++ b/modules/Oculus_module/CMakeLists.txt @@ -41,22 +41,12 @@ yarp_add_idl(${EXE_TARGET_NAME}_THRIFT_GEN_FILES ${${EXE_TARGET_NAME}_THRIFT_HDR add_executable(${EXE_TARGET_NAME} ${${EXE_TARGET_NAME}_SRC} ${${EXE_TARGET_NAME}_HDR} ${${EXE_TARGET_NAME}_THRIFT_GEN_FILES}) -if(ENABLE_LOGGER) - target_link_libraries(${EXE_TARGET_NAME} LINK_PUBLIC - ${YARP_LIBRARIES} - ${iDynTree_LIBRARIES} - ctrlLib - UtilityLibrary - Eigen3::Eigen - matlogger2::matlogger2) -else(ENABLE_LOGGER) - target_link_libraries(${EXE_TARGET_NAME} LINK_PUBLIC +target_link_libraries(${EXE_TARGET_NAME} LINK_PUBLIC ${YARP_LIBRARIES} ${iDynTree_LIBRARIES} ctrlLib UtilityLibrary Eigen3::Eigen ) -endif() install(TARGETS ${EXE_TARGET_NAME} DESTINATION bin) diff --git a/modules/Oculus_module/include/OculusModule.hpp b/modules/Oculus_module/include/OculusModule.hpp index 1f3f3f3c..df174a34 100644 --- a/modules/Oculus_module/include/OculusModule.hpp +++ b/modules/Oculus_module/include/OculusModule.hpp @@ -26,11 +26,6 @@ #include -#ifdef ENABLE_LOGGER -#include -#include -#endif - /** * OculusModule is the main core of the Oculus application. It is goal is to evaluate retrieve the * Oculus readouts, send the desired pose of the hands to the walking application, move the robot @@ -150,15 +145,8 @@ class OculusModule : public yarp::os::RFModule, public TeleoperationCommands Virtualizer) only yaw. */ double m_playerOrientationThreshold; /**< Player orientation threshold. */ - bool m_enableLogger; /**< log the data (if ON) */ - std::mutex m_mutex; /**< Mutex. */ -#ifdef ENABLE_LOGGER - XBot::MatLogger2::Ptr m_logger; /**< */ - XBot::MatAppender::Ptr m_appender; - std::string m_logger_prefix{"oculus"}; -#endif /** * Configure the Oculus. * @param config configuration object @@ -222,12 +210,6 @@ class OculusModule : public yarp::os::RFModule, public TeleoperationCommands */ bool getTransforms(); - /** - * Open the logger - * @return true if it could open the logger - */ - bool openLogger(); - public: OculusModule(); ~OculusModule(); diff --git a/modules/Oculus_module/src/OculusModule.cpp b/modules/Oculus_module/src/OculusModule.cpp index 126aa7be..28400a14 100644 --- a/modules/Oculus_module/src/OculusModule.cpp +++ b/modules/Oculus_module/src/OculusModule.cpp @@ -388,12 +388,6 @@ bool OculusModule::configure(yarp::os::ResourceFinder& rf) { std::lock_guard guard(m_mutex); -#ifdef ENABLE_LOGGER - yInfo() << "[OculusModule::configure] matlogger2 is eanbled!"; -#else - yInfo() << "[OculusModule::configure] matlogger2 is disabled!"; -#endif - // check if the configuration file is empty if (rf.isNull()) { @@ -416,9 +410,6 @@ bool OculusModule::configure(yarp::os::ResourceFinder& rf) yInfo() << "[OculusModule::configure] player orientation threshold: " << m_playerOrientationThreshold; - // check if log the data - m_enableLogger = generalOptions.check("enableLogger", yarp::os::Value(0)).asBool(); - // set the module name std::string name; if (!YarpHelper::getStringFromSearchable(rf, "name", name)) @@ -596,15 +587,6 @@ bool OculusModule::configure(yarp::os::ResourceFinder& rf) m_playerOrientationOld = 0; m_robotYaw = 0; - // open the logger only if all the vecotos sizes are clear. - if (m_enableLogger) - { - if (!openLogger()) - { - yError() << "[OculusModule::configure] Unable to open the logger"; - return false; - } - } m_oculusHeadsetPoseInertial.resize(6, 0.0); // Reset the cameras if necessary @@ -699,14 +681,6 @@ bool OculusModule::close() { std::lock_guard guard(m_mutex); -#ifdef ENABLE_LOGGER - if (m_enableLogger) - { - m_logger->flush_available_data(); - } -#endif - // m_logger.reset(); - // close devices if (!m_useXsens) { @@ -1247,78 +1221,6 @@ bool OculusModule::updateModule() yInfo() << "[OculusModule::updateModule] stop"; return false; } - -#ifdef ENABLE_LOGGER - if (m_enableLogger) - { - m_logger->add(m_logger_prefix + "_time", yarp::os::Time::now()); - m_logger->add(m_logger_prefix + "_playerOrientation", m_playerOrientation); - if (m_moveRobot) - { - m_logger->add(m_logger_prefix + "_robotYaw", m_robotYaw); - } else - { - m_logger->add(m_logger_prefix + "_robotYaw", 0.0); - } - - std::vector neckAngles; - yarp::sig::Vector neckValuesSig; - m_head->getNeckJointValues(neckValuesSig); - for (unsigned i = 0; i < neckValuesSig.size(); i++) - { - neckAngles.push_back(neckValuesSig(i)); - } - m_logger->add(m_logger_prefix + "_neckJointValues", neckAngles); - - std::vector lFingers, rFingers; - m_leftHandFingers->getFingerValues(lFingers); - m_rightHandFingers->getFingerValues(rFingers); - if (lFingers.size() > 0) - { - m_logger->add(m_logger_prefix + "_leftFingerValues", lFingers); - } - if (rFingers.size() > 0) - { - m_logger->add(m_logger_prefix + "_rightFingerValues", rFingers); - } - - std::vector left_robotHandpose_robotTel, left_humanHandpose_oculusInertial, - left_humanHandpose_humanTel; - m_leftHand->getHandInfo(left_robotHandpose_robotTel, - left_humanHandpose_oculusInertial, - left_humanHandpose_humanTel); - - m_logger->add(m_logger_prefix + "_left_robotHandpose_robotTeleoperation", - left_robotHandpose_robotTel); - m_logger->add(m_logger_prefix + "_left_humanHandpose_oculusInertial", - left_humanHandpose_oculusInertial); - m_logger->add(m_logger_prefix + "_left_humanHandpose_humanTeleoperation", - left_humanHandpose_humanTel); - - std::vector right_robotHandpose_robotTel, right_humanHandpose_oculusInertial, - right_humanHandpose_humanTel; - m_rightHand->getHandInfo(right_robotHandpose_robotTel, - right_humanHandpose_oculusInertial, - right_humanHandpose_humanTel); - - m_logger->add(m_logger_prefix + "_right_robotHandpose_robotTeleoperation", - right_robotHandpose_robotTel); - m_logger->add(m_logger_prefix + "_right_humanHandpose_oculusInertial", - right_humanHandpose_oculusInertial); - m_logger->add(m_logger_prefix + "_right_humanHandpose_humanTeleoperation", - right_humanHandpose_humanTel); - - m_logger->add(m_logger_prefix + "_oculusHeadset_Inertial", - m_oculusHeadsetPoseInertial); // pose sizein 3D space - - if (!m_useVirtualizer) - { - m_logger->add(m_logger_prefix + "_loc_joypad_x_y", locCmd); - } - - m_logger->flush_available_data(); - } -#endif } else if (m_state == OculusFSM::Configured) { // check if it is time to prepare or start walking @@ -1382,54 +1284,6 @@ double OculusModule::deadzone(const double& input) } } -bool OculusModule::openLogger() -{ -#ifdef ENABLE_LOGGER - std::string currentTime = YarpHelper::getTimeDateMatExtension(); - std::string fileName = "OculusModule" + currentTime + "log.mat"; - - m_logger = XBot::MatLogger2::MakeLogger(fileName); - m_appender = XBot::MatAppender::MakeInstance(); - m_appender->add_logger(m_logger); - m_appender->start_flush_thread(); - - m_logger->create(m_logger_prefix + "_time", 1); - m_logger->create(m_logger_prefix + "_playerOrientation", 1); - - m_logger->create(m_logger_prefix + "_robotYaw", 1); - - m_logger->create(m_logger_prefix + "_neckJointValues", m_head->controlHelper()->getDoFs()); - m_logger->create(m_logger_prefix + "_leftFingerValues", - m_leftHandFingers->controlHelper()->getDoFs()); - m_logger->create(m_logger_prefix + "_rightFingerValues", - m_rightHandFingers->controlHelper()->getDoFs()); - - m_logger->create(m_logger_prefix + "_left_robotHandpose_robotTeleoperation", - 6); // pose sizein 3D space - m_logger->create(m_logger_prefix + "_left_humanHandpose_oculusInertial", - 6); // pose sizein 3D space - m_logger->create(m_logger_prefix + "_left_humanHandpose_humanTeleoperation", - 6); // pose sizein 3D space - - m_logger->create(m_logger_prefix + "_right_robotHandpose_robotTeleoperation", - 6); // pose sizein 3D space - m_logger->create(m_logger_prefix + "_right_humanHandpose_oculusInertial", - 6); // pose sizein 3D space - m_logger->create(m_logger_prefix + "_right_humanHandpose_humanTeleoperation", - 6); // pose sizein 3D space - m_logger->create(m_logger_prefix + "_oculusHeadset_Inertial", - 6); // pose sizein 3D space - - m_logger->create(m_logger_prefix + "_loc_joypad_x_y", - 2); // [x,y] component for robot locomotion - yInfo() << "[OculusModule::openLogger] Logging is active."; -#else - yInfo() << "[OculusModule::openLogger] option is not active in CMakeLists."; - -#endif - return true; -} - void OculusModule::Impl::initializeNeckJointsSmoother(const unsigned m_actuatedDOFs, const double m_dT, const double smoothingTime, From 33331301c737b3bffd7c56d57bcb10fa6b802078 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 30 Jul 2024 19:13:54 +0200 Subject: [PATCH 05/11] Initialize the m_n variable in RobotMotorsEstimation See https://github.com/robotology/walking-teleoperation/issues/145 --- modules/HapticGlove_module/src/RobotMotorsEstimation.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/modules/HapticGlove_module/src/RobotMotorsEstimation.cpp b/modules/HapticGlove_module/src/RobotMotorsEstimation.cpp index 9e74ae52..52e5f4d8 100644 --- a/modules/HapticGlove_module/src/RobotMotorsEstimation.cpp +++ b/modules/HapticGlove_module/src/RobotMotorsEstimation.cpp @@ -66,6 +66,7 @@ bool Estimators::configure(const yarp::os::Searchable& config, const std::string } m_z = Eigen::MatrixXd::Zero(no_measurement_kf, 1); + m_n = 3; // joint/motor position, velocity, acceleration m_x_hat = Eigen::MatrixXd::Zero(no_measurement_kf, 1); m_P.resize(no_states_kf * no_states_kf); From 717f39da5d15e7a9f71c4bc8530a9945619aa705 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 1 Aug 2024 11:21:10 +0200 Subject: [PATCH 06/11] Using bipedal as optional dependency --- cmake/WalkingTeleoperationFindDependencies.cmake | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/cmake/WalkingTeleoperationFindDependencies.cmake b/cmake/WalkingTeleoperationFindDependencies.cmake index 2291c151..62a4adaf 100644 --- a/cmake/WalkingTeleoperationFindDependencies.cmake +++ b/cmake/WalkingTeleoperationFindDependencies.cmake @@ -151,9 +151,13 @@ checkandset_dependency(IWear) find_package(WearableActuators 1.9.0 QUIET) checkandset_dependency(WearableActuators) +find_package(BipedalLocomotionFramework 0.18.0 + COMPONENTS VectorsCollection ParametersHandlerYarpImplementation QUIET) +checkandset_dependency(BipedalLocomotionFramework) WALKING_TELEOPERATION_dependent_option(WALKING_TELEOPERATION_COMPILE_XsensModule "Compile Xsens Module?" ON WALKING_TELEOPERATION_HAS_HumanDynamicsEstimation OFF) WALKING_TELEOPERATION_dependent_option(WALKING_TELEOPERATION_COMPILE_VirtualizerModule "Compile Virtualizer Module?" ON WALKING_TELEOPERATION_HAS_CybSDK OFF) WALKING_TELEOPERATION_dependent_option(WALKING_TELEOPERATION_COMPILE_FaceExpressionsRetargetingModule "Compile Face Expressions Module?" ON WALKING_TELEOPERATION_USE_libfvad OFF) WALKING_TELEOPERATION_dependent_option(WALKING_TELEOPERATION_COMPILE_SRanipalModule "Compile SRanipal Module?" ON WALKING_TELEOPERATION_USE_SRanipalSDK OFF) -WALKING_TELEOPERATION_dependent_option(WALKING_TELEOPERATION_COMPILE_HapticGloveModule "Compile Haptic Glove Module?" ON "WALKING_TELEOPERATION_USE_IWear;WALKING_TELEOPERATION_USE_WearableActuators" OFF) +WALKING_TELEOPERATION_dependent_option(WALKING_TELEOPERATION_COMPILE_HapticGloveModule "Compile Haptic Glove Module?" ON + "WALKING_TELEOPERATION_USE_IWear;WALKING_TELEOPERATION_USE_WearableActuators;WALKING_TELEOPERATION_USE_BipedalLocomotionFramework" OFF) From 7db14273139eb1e288a0d09074ae763326253b24 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 1 Aug 2024 12:12:56 +0200 Subject: [PATCH 07/11] Return directly the vector of names for human and controlled robot joints This simplifies the logger preparation --- .../include/GloveControlHelper.hpp | 10 ++++----- .../include/RobotInterface.hpp | 4 ++-- .../src/GloveControlHelper.cpp | 22 ++++--------------- .../src/RobotController.cpp | 6 ++--- .../HapticGlove_module/src/RobotInterface.cpp | 8 +++---- .../HapticGlove_module/src/Teleoperation.cpp | 10 +++------ 6 files changed, 20 insertions(+), 40 deletions(-) diff --git a/modules/HapticGlove_module/include/GloveControlHelper.hpp b/modules/HapticGlove_module/include/GloveControlHelper.hpp index e2920972..776e9277 100644 --- a/modules/HapticGlove_module/include/GloveControlHelper.hpp +++ b/modules/HapticGlove_module/include/GloveControlHelper.hpp @@ -162,10 +162,9 @@ class HapticGlove::GloveControlHelper /** * Get the list of the human hand joint names - * @param jointNameList human hand joints name list - * @return true/false in case of success/failure + * @return human hand joints name list */ - bool getHumanHandJointsNames(std::vector& jointNameList) const; + const std::vector& getHumanHandJointsNames() const; /** * Get the name of a finger given the index of the finger in the list @@ -177,10 +176,9 @@ class HapticGlove::GloveControlHelper /** * Get the list of the human finger names - * @param fingerNameList human hand fingers name list - * @return true/false in case of success/failure + * @return human hand fingers name list */ - bool getHumanHandFingerNames(std::vector& fingerNameList) const; + const std::vector& getHumanHandFingerNames() const; /** * Find the range of the motion of the human finger joints of the hand diff --git a/modules/HapticGlove_module/include/RobotInterface.hpp b/modules/HapticGlove_module/include/RobotInterface.hpp index 7922e0e7..9baaa926 100644 --- a/modules/HapticGlove_module/include/RobotInterface.hpp +++ b/modules/HapticGlove_module/include/RobotInterface.hpp @@ -548,7 +548,7 @@ class HapticGlove::RobotInterface * Get the name of the actuated joints * @param names the names of the actuated joints */ - void getActuatedJointNames(std::vector& names) const; + const std::vector& getActuatedJointNames() const; /** * Get the name of the all the joints related to the used parts (robot hand) @@ -560,7 +560,7 @@ class HapticGlove::RobotInterface * Get the name of the actuated axes * @param names the names of the actuated axes */ - void getActuatedAxisNames(std::vector& names) const; + const std::vector& getActuatedAxisNames() const; /** * Get the name of the all the axes related to the used parts (robot hand) diff --git a/modules/HapticGlove_module/src/GloveControlHelper.cpp b/modules/HapticGlove_module/src/GloveControlHelper.cpp index ab6881b6..2d2dc565 100644 --- a/modules/HapticGlove_module/src/GloveControlHelper.cpp +++ b/modules/HapticGlove_module/src/GloveControlHelper.cpp @@ -318,16 +318,9 @@ bool GloveControlHelper::getHumanHandJointName(const size_t i, std::string& join return true; } -bool GloveControlHelper::getHumanHandJointsNames(std::vector& jointNameList) const +const std::vector& GloveControlHelper::getHumanHandJointsNames() const { - if (m_humanJointNameList.size() != this->getNumOfHandJoints()) - { - yError() << m_logPrefix - << "The number of human hand joints and joints name list size are different."; - return false; - } - jointNameList = m_humanJointNameList; - return true; + return m_humanJointNameList; } bool GloveControlHelper::getHumanHandFingerName(const size_t i, std::string& fingerName) const @@ -342,16 +335,9 @@ bool GloveControlHelper::getHumanHandFingerName(const size_t i, std::string& fin return true; } -bool GloveControlHelper::getHumanHandFingerNames(std::vector& fingerNameList) const +const std::vector& GloveControlHelper::getHumanHandFingerNames() const { - if (m_humanFingerNameList.size() != this->getNumOfFingers()) - { - yError() << m_logPrefix - << "The number of human hand finger and finger name list size are different."; - return false; - } - fingerNameList = m_humanFingerNameList; - return true; + return m_humanFingerNameList; } bool GloveControlHelper::findHumanMotionRange() diff --git a/modules/HapticGlove_module/src/RobotController.cpp b/modules/HapticGlove_module/src/RobotController.cpp index 9b920895..7838ad59 100644 --- a/modules/HapticGlove_module/src/RobotController.cpp +++ b/modules/HapticGlove_module/src/RobotController.cpp @@ -122,11 +122,11 @@ bool RobotController::configure(const yarp::os::Searchable& config, // get control gains from configuration files std::vector allAxisNames, actuatedAxisNames; - std::vector allJointNames, actuatedJointNames; + std::vector allJointNames; m_robotInterface->getAllAxisNames(allAxisNames); - m_robotInterface->getAllAxisNames(actuatedAxisNames); + m_robotInterface->getAllAxisNames(actuatedAxisNames); //TODO: Bug? Should be getActuatedAxisNames m_robotInterface->getAllJointNames(allJointNames); - m_robotInterface->getActuatedJointNames(actuatedJointNames); + const std::vector & actuatedJointNames = m_robotInterface->getActuatedJointNames(); std::vector q, r; std::vector qTmp, rTmp; diff --git a/modules/HapticGlove_module/src/RobotInterface.cpp b/modules/HapticGlove_module/src/RobotInterface.cpp index b58048a6..b965b669 100644 --- a/modules/HapticGlove_module/src/RobotInterface.cpp +++ b/modules/HapticGlove_module/src/RobotInterface.cpp @@ -1082,9 +1082,9 @@ void RobotInterface::getFingerNames(std::vector& names) const names = m_robotFingerNames; } -void RobotInterface::getActuatedJointNames(std::vector& names) const +const std::vector& RobotInterface::getActuatedJointNames() const { - names = m_actuatedJointList; + return m_actuatedJointList; } void RobotInterface::getAllJointNames(std::vector& names) const @@ -1092,9 +1092,9 @@ void RobotInterface::getAllJointNames(std::vector& names) const names = m_allJointNames; } -void RobotInterface::getActuatedAxisNames(std::vector& names) const +const std::vector& RobotInterface::getActuatedAxisNames() const { - names = m_actuatedAxisNames; + return m_actuatedAxisNames; } void RobotInterface::getAllAxisNames(std::vector& names) const diff --git a/modules/HapticGlove_module/src/Teleoperation.cpp b/modules/HapticGlove_module/src/Teleoperation.cpp index 959f2e7c..3d4cea3b 100644 --- a/modules/HapticGlove_module/src/Teleoperation.cpp +++ b/modules/HapticGlove_module/src/Teleoperation.cpp @@ -66,13 +66,9 @@ bool Teleoperation::configure(const yarp::os::Searchable& config, } // initialize the retaregting object - std::vector robotActuatedJointNameList; - std::vector robotActuatedAxisNameList; - std::vector humanJointNameList; - - m_robotController->controlHelper()->getActuatedJointNames(robotActuatedJointNameList); - m_robotController->controlHelper()->getActuatedAxisNames(robotActuatedAxisNameList); - m_humanGlove->getHumanHandJointsNames(humanJointNameList); + const std::vector& robotActuatedJointNameList = m_robotController->controlHelper()->getActuatedJointNames(); + const std::vector& robotActuatedAxisNameList = m_robotController->controlHelper()->getActuatedAxisNames(); + const std::vector& humanJointNameList = m_humanGlove->getHumanHandJointsNames(); m_retargeting = std::make_unique( robotActuatedJointNameList, robotActuatedAxisNameList, humanJointNameList); From 6f6b5b9317f9292ebb8ab68fadc6b1a66af8973d Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 1 Aug 2024 12:13:45 +0200 Subject: [PATCH 08/11] Added possibility to get fingersTactileData --- .../HapticGlove_module/include/RobotSkin.hpp | 6 +- modules/HapticGlove_module/src/RobotSkin.cpp | 57 ++++++++++--------- 2 files changed, 35 insertions(+), 28 deletions(-) diff --git a/modules/HapticGlove_module/include/RobotSkin.hpp b/modules/HapticGlove_module/include/RobotSkin.hpp index 6f247c6d..ae0872a2 100644 --- a/modules/HapticGlove_module/include/RobotSkin.hpp +++ b/modules/HapticGlove_module/include/RobotSkin.hpp @@ -113,7 +113,7 @@ struct HapticGlove::FingertipTactileData double contactThreshold() { - return contactThresholdValue + return contactThresholdValue + contactThresholdMultiplier * stdTactileSensor[this->maxTactileFeedbackAbsoluteElement()]; } @@ -205,7 +205,7 @@ class HapticGlove::RobotSkin : RobotSkinService // RPC port yarp::os::Port m_rpcPort; - + void updateCalibratedTactileData(); void computeVibrotactileFeedback(); @@ -301,6 +301,8 @@ class HapticGlove::RobotSkin : RobotSkinService virtual bool setDerivativeThresholdMultiplierAll(const double value) override; + const std::vector& getFingersTactileData() const; + bool close(); }; diff --git a/modules/HapticGlove_module/src/RobotSkin.cpp b/modules/HapticGlove_module/src/RobotSkin.cpp index cd989036..2d7180ad 100644 --- a/modules/HapticGlove_module/src/RobotSkin.cpp +++ b/modules/HapticGlove_module/src/RobotSkin.cpp @@ -438,7 +438,7 @@ void RobotSkin::computeVibrotactileFeedback() double vibrotactileDerivativeFeedback = m_fingersTactileData[i].vibrotactileDerivativeGain * (m_areFingersContactChanges[i] ? std::abs(m_fingersContactStrengthDerivateSmoothed[i]): 0); - + m_fingersVibrotactileDerivativeFeedback[i] = m_fbParams[0] * std::log(m_fbParams[1] * std::pow(vibrotactileDerivativeFeedback, m_fbParams[2]) + m_fbParams[3]) + m_fbParams[4] * std::pow(vibrotactileDerivativeFeedback, m_fbParams[5]); @@ -595,8 +595,8 @@ bool RobotSkin::setAbsoluteSkinValuePercentage(const double value) std::lock_guard lock(m_mutex); if(value<0.0 || value > 1.0 ) { - yWarning() << m_logPrefix - << "Requested setAbsoluteSkinValuePercentage with unfeasible value " + yWarning() << m_logPrefix + << "Requested setAbsoluteSkinValuePercentage with unfeasible value " << value << "!"; return false; } @@ -610,8 +610,8 @@ bool RobotSkin::setSkinDerivativeSmoothingGain(const double value) std::lock_guard lock(m_mutex); if(value<0.0 || value > 1.0 ) { - yWarning() << m_logPrefix - << "Requested setSkinDerivativeSmoothingGain with unfeasible value " + yWarning() << m_logPrefix + << "Requested setSkinDerivativeSmoothingGain with unfeasible value " << value << "!"; return false; } @@ -625,15 +625,15 @@ bool RobotSkin::setContactFeedbackGain(const int32_t finger, const double value) std::lock_guard lock(m_mutex); if(value<0.0) { - yWarning() << m_logPrefix - << "Requested setContactFeedbackGain with unfeasible value " + yWarning() << m_logPrefix + << "Requested setContactFeedbackGain with unfeasible value " << value << "!"; return false; } if (finger >= m_noFingers) { - yWarning() << m_logPrefix - << "Requested setContactFeedbackGain with invalid finger index " + yWarning() << m_logPrefix + << "Requested setContactFeedbackGain with invalid finger index " << finger << "!"; return false; } @@ -647,8 +647,8 @@ bool RobotSkin::setContactFeedbackGainAll(const double value) std::lock_guard lock(m_mutex); if(value<0.0) { - yWarning() << m_logPrefix - << "Requested setContactFeedbackGainAll with unfeasible value " + yWarning() << m_logPrefix + << "Requested setContactFeedbackGainAll with unfeasible value " << value << "!"; return false; } @@ -657,7 +657,7 @@ bool RobotSkin::setContactFeedbackGainAll(const double value) { m_fingersTactileData[i].vibrotactileGain = value; } - + return true; } @@ -666,15 +666,15 @@ bool RobotSkin::setDerivativeFeedbackGain(const int32_t finger, const double val std::lock_guard lock(m_mutex); if(value<0.0) { - yWarning() << m_logPrefix - << "Requested setDerivativeFeedbackGain with unfeasible value " + yWarning() << m_logPrefix + << "Requested setDerivativeFeedbackGain with unfeasible value " << value << "!"; return false; } if (finger >= m_noFingers) { - yWarning() << m_logPrefix - << "Requested setDerivativeFeedbackGain with invalid finger index " + yWarning() << m_logPrefix + << "Requested setDerivativeFeedbackGain with invalid finger index " << finger << "!"; return false; } @@ -688,8 +688,8 @@ bool RobotSkin::setDerivativeFeedbackGainAll(const double value) std::lock_guard lock(m_mutex); if(value<0.0) { - yWarning() << m_logPrefix - << "Requested setDerivativeFeedbackGainAll with unfeasible value " + yWarning() << m_logPrefix + << "Requested setDerivativeFeedbackGainAll with unfeasible value " << value << "!"; return false; } @@ -707,8 +707,8 @@ bool RobotSkin::setContactThreshold(const int32_t finger, const double value) std::lock_guard lock(m_mutex); if (finger >= m_noFingers) { - yWarning() << m_logPrefix - << "Requested setContactThreshold with invalid finger index " + yWarning() << m_logPrefix + << "Requested setContactThreshold with invalid finger index " << finger << "!"; return false; } @@ -734,8 +734,8 @@ bool RobotSkin::setContactThresholdMultiplier(const int32_t finger, const double std::lock_guard lock(m_mutex); if (finger >= m_noFingers) { - yWarning() << m_logPrefix - << "Requested setContactThresholdMultiplier with invalid finger index " + yWarning() << m_logPrefix + << "Requested setContactThresholdMultiplier with invalid finger index " << finger << "!"; return false; } @@ -761,8 +761,8 @@ bool RobotSkin::setDerivativeThreshold(const int32_t finger, const double value) std::lock_guard lock(m_mutex); if (finger >= m_noFingers) { - yWarning() << m_logPrefix - << "Requested setDerivativeThreshold with invalid finger index " + yWarning() << m_logPrefix + << "Requested setDerivativeThreshold with invalid finger index " << finger << "!"; return false; } @@ -788,8 +788,8 @@ bool RobotSkin::setDerivativeThresholdMultiplier(const int32_t finger, const dou std::lock_guard lock(m_mutex); if (finger >= m_noFingers) { - yWarning() << m_logPrefix - << "Requested setDerivativeThresholdMultiplier with invalid finger index " + yWarning() << m_logPrefix + << "Requested setDerivativeThresholdMultiplier with invalid finger index " << finger << "!"; return false; } @@ -809,3 +809,8 @@ bool RobotSkin::setDerivativeThresholdMultiplierAll(const double value) return true; } + +const std::vector& HapticGlove::RobotSkin::getFingersTactileData() const +{ + return m_fingersTactileData; +} From 2e6d978d3226b0a4d6b9b985c4e447d786a6ef7f Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 1 Aug 2024 12:16:04 +0200 Subject: [PATCH 09/11] Added possibility to retrieve more data from the teleoperation object --- .../include/Teleoperation.hpp | 32 +++++++++++++++++++ .../HapticGlove_module/src/Teleoperation.cpp | 30 +++++++++++++++++ 2 files changed, 62 insertions(+) diff --git a/modules/HapticGlove_module/include/Teleoperation.hpp b/modules/HapticGlove_module/include/Teleoperation.hpp index 83b3eded..e298a478 100644 --- a/modules/HapticGlove_module/include/Teleoperation.hpp +++ b/modules/HapticGlove_module/include/Teleoperation.hpp @@ -259,6 +259,38 @@ class HapticGlove::Teleoperation : HapticGloveTeleoperationService * @param time the time at which the configuration is ended */ void setEndOfConfigurationTime(const double& time); + + /** + * Get the names of the robot actuated axis + */ + const std::vector& getActuatedAxisNames() const; + + /** + * Get the names of the robot actuated joints + */ + const std::vector& getActuatedJointNames() const; + + /** + * Get the names of the human hand joints + */ + const std::vector& getHumanHandJointsNames() const; + + /** + * Get the names of the human hand fingers + */ + const std::vector& getHumanHandFingerNames() const; + + /** + * Get the teleoperation data + */ + const Data& getData() const; + + /** + * Get the skin data + */ + const std::vector& getFingersTactileData() const; + + }; #endif // TELEOPERATION_HPP diff --git a/modules/HapticGlove_module/src/Teleoperation.cpp b/modules/HapticGlove_module/src/Teleoperation.cpp index 3d4cea3b..e5083ff7 100644 --- a/modules/HapticGlove_module/src/Teleoperation.cpp +++ b/modules/HapticGlove_module/src/Teleoperation.cpp @@ -528,3 +528,33 @@ void Teleoperation::setEndOfConfigurationTime(const double& time) { m_timeConfigurationEnd = time; } + +const std::vector& Teleoperation::getActuatedAxisNames() const +{ + return m_robotController->controlHelper()->getActuatedAxisNames(); +} + +const std::vector& Teleoperation::getActuatedJointNames() const +{ + return m_robotController->controlHelper()->getActuatedJointNames(); +} + +const std::vector& HapticGlove::Teleoperation::getHumanHandJointsNames() const +{ + return m_humanGlove->getHumanHandJointsNames(); +} + +const std::vector& HapticGlove::Teleoperation::getHumanHandFingerNames() const +{ + return m_humanGlove->getHumanHandFingerNames(); +} + +const Data& HapticGlove::Teleoperation::getData() const +{ + return m_data; +} + +const std::vector& HapticGlove::Teleoperation::getFingersTactileData() const +{ + return m_robotSkin->getFingersTactileData(); +} From 879a7dcc359c48a7b851700e8683f29e49b3de70 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 1 Aug 2024 12:17:04 +0200 Subject: [PATCH 10/11] Using bipedal vectors collection server to log data in HapticGloveModule --- modules/HapticGlove_module/CMakeLists.txt | 4 +- .../include/HapticGloveModule.hpp | 9 + modules/HapticGlove_module/include/Logger.hpp | 63 +-- .../include/Teleoperation.hpp | 16 +- .../src/HapticGloveModule.cpp | 70 ++- modules/HapticGlove_module/src/Logger.cpp | 535 +++++------------- .../HapticGlove_module/src/Teleoperation.cpp | 56 +- 7 files changed, 300 insertions(+), 453 deletions(-) diff --git a/modules/HapticGlove_module/CMakeLists.txt b/modules/HapticGlove_module/CMakeLists.txt index e29ec4ce..1172a5db 100644 --- a/modules/HapticGlove_module/CMakeLists.txt +++ b/modules/HapticGlove_module/CMakeLists.txt @@ -12,7 +12,7 @@ IF(EIGEN_RUNTIME_NO_MALLOC) ADD_DEFINITIONS(-DEIGEN_RUNTIME_NO_MALLOC) ENDIF(EIGEN_RUNTIME_NO_MALLOC) -yarp_add_idl(HAPTIC_GLOVE_SERVICES thrift/RobotSkinService.thrift +yarp_add_idl(HAPTIC_GLOVE_SERVICES thrift/RobotSkinService.thrift thrift/HapticGloveTeleoperationService.thrift) # set cpp files @@ -69,6 +69,8 @@ set(${EXE_TARGET_NAME}_LINKED_LIBS UtilityLibrary IWear::IWear WearableActuators::WearableActuators + BipedalLocomotion::VectorsCollection + BipedalLocomotion::ParametersHandlerYarpImplementation ) target_link_libraries(${EXE_TARGET_NAME} LINK_PUBLIC ${${EXE_TARGET_NAME}_LINKED_LIBS} ) diff --git a/modules/HapticGlove_module/include/HapticGloveModule.hpp b/modules/HapticGlove_module/include/HapticGloveModule.hpp index a4669867..bb047082 100644 --- a/modules/HapticGlove_module/include/HapticGloveModule.hpp +++ b/modules/HapticGlove_module/include/HapticGloveModule.hpp @@ -13,6 +13,10 @@ // teleoperation #include +// BipedalLocomotion +#include + + /** * HapticGloveModule is the main core application of the bilateral teleoperation of the human hand * and robot hand . It is goal is to evaluate retrieve the sense glove readouts, send the desired @@ -45,6 +49,11 @@ class HapticGloveModule : public yarp::os::RFModule std::unique_ptr m_leftHand; std::unique_ptr m_rightHand; + bool m_enableLogger; /**< True if data are saved. */ + + BipedalLocomotion::YarpUtilities::VectorsCollectionServer m_vectorsCollectionServer; /**< Logger server. */ + + public: /** * Constructor diff --git a/modules/HapticGlove_module/include/Logger.hpp b/modules/HapticGlove_module/include/Logger.hpp index bbd39bcd..bdf463a5 100644 --- a/modules/HapticGlove_module/include/Logger.hpp +++ b/modules/HapticGlove_module/include/Logger.hpp @@ -10,11 +10,16 @@ // teleoperation #include -// matlogger -#ifdef ENABLE_LOGGER -#include -#include -#endif +// blf +#include + +struct LoggerOptions +{ + bool isRightHand = false; /// check if the right hand + bool dumpSkinData = false; /// check if dumping skin + bool dumpHumanData = false; /// check if dumping human data + bool dumpKalmanFilterData = false; /// check if dumping kalman filter data +}; /** * Logger Class useful for logging all the relevant information for the haptic glove teleoperation @@ -22,41 +27,16 @@ */ class HapticGlove::Teleoperation::Logger { - bool m_isRightHand; /// check if the left or right hand std::string m_handName; /// the hand name (.eg., left or right) - std::string m_logPrefix; /// logging prefix - bool m_useSkin; /// check if using skin + LoggerOptions m_options; /// the logger options + + std::vector m_fingerSkinContactBuffer; /// A buffer for the finger skin contact to convert from bool const Teleoperation& m_teleoperation; /// the constant reference to the parent teleoperation object - size_t m_numRobotActuatedAxes; /// the number of robot actuated axis - size_t m_numRobotActuatedJoints; /// the number of robot actuated joints - - size_t m_numHumanHandJoints; /// the number of human hand joints - size_t m_numHumanHandFingers; /// the number of human hand finger - size_t m_numHumanVibrotactileFeedback; /// the number of vibrotactile feedbacks to the - /// user - size_t m_numHumanForceFeedback; /// the number of force feedback to the user - size_t m_numberRobotTactileFeedbacks; /// the number of tactile feedback of the robot - /// fingertips - - std::string m_robotPrefix; /// robot prefix for logging - std::string m_humanPrefix; /// human prefix for logging - std::string m_logFileName; /// the file name where the data is saved - - Data m_data; /// the data structure - -#ifdef ENABLE_LOGGER - XBot::MatLogger2::Ptr m_logger; /**< the pointer to the logger */ - XBot::MatAppender::Ptr m_appender; -#endif - - /** - * update the data structure of the logger - * */ - bool updateData(); + BipedalLocomotion::YarpUtilities::VectorsCollectionServer& m_logger; /**< the pointer to the logger */ public: /** @@ -64,27 +44,20 @@ class HapticGlove::Teleoperation::Logger * @param module a constant reference to the parent teleoperation object * @param isRightHand check if the right hand or the left hand */ - Logger(const Teleoperation& module, const bool isRightHand); + Logger(const Teleoperation& module, + LoggerOptions options, + BipedalLocomotion::YarpUtilities::VectorsCollectionServer& loggerObject); /** * Destructor * */ - ~Logger(); - - /** - * open the logger - * */ - bool openLogger(); + ~Logger() = default; /** * log the data * */ bool logData(); - /** - * close the logger - * */ - bool closeLogger(); }; #endif // LOGGER_HPP diff --git a/modules/HapticGlove_module/include/Teleoperation.hpp b/modules/HapticGlove_module/include/Teleoperation.hpp index e298a478..0d5736da 100644 --- a/modules/HapticGlove_module/include/Teleoperation.hpp +++ b/modules/HapticGlove_module/include/Teleoperation.hpp @@ -26,6 +26,13 @@ // rpc service #include +// blf +#include + +//std +#include +#include + namespace HapticGlove { class Teleoperation; @@ -192,7 +199,7 @@ class HapticGlove::Teleoperation : HapticGloveTeleoperationService // Enable at the end bool m_enableLogger; /**< log the data (if true) */ class Logger; /**< forward decleration of the logger class */ - std::unique_ptr m_loggerLeftHand; /**< pointer to the logger object. */ + std::unique_ptr m_logger; /**< pointer to the logger object. */ // mutex std::mutex m_mutex; @@ -224,10 +231,13 @@ class HapticGlove::Teleoperation : HapticGloveTeleoperationService * @param config configuration options * @param name name of the robot * @param rightHand if true the right hand is used + * @param loggerObject the logger object, used by both hands * @return true/false in case of success/failure */ - bool - configure(const yarp::os::Searchable& config, const std::string& name, const bool& rightHand); + bool configure(const yarp::os::Searchable& config, + const std::string& name, + const bool& rightHand, + BipedalLocomotion::YarpUtilities::VectorsCollectionServer& loggerObject); /** * Close the teleoperation class. diff --git a/modules/HapticGlove_module/src/HapticGloveModule.cpp b/modules/HapticGlove_module/src/HapticGloveModule.cpp index f2d3f1b3..f3a430e1 100644 --- a/modules/HapticGlove_module/src/HapticGloveModule.cpp +++ b/modules/HapticGlove_module/src/HapticGloveModule.cpp @@ -11,6 +11,10 @@ #include #include +// blf +#include +#include + HapticGloveModule::HapticGloveModule() { m_logPrefix = "HapticGloveModule:: "; @@ -50,14 +54,52 @@ bool HapticGloveModule::configure(yarp::os::ResourceFinder& rf) yInfo() << m_logPrefix << "use the left hand: " << m_useLeftHand; yInfo() << m_logPrefix << "use the right hand: " << m_useRightHand; + m_enableLogger = generalOptions.check("enableLogger", yarp::os::Value(false)).asBool(); + yInfo() << m_logPrefix << "enable logger: " << m_enableLogger; + + // initialize the logger + if (m_enableLogger) + { + auto loggerOption + = std::make_shared(rf) + ->getGroup("LOGGER") + .lock(); + if (loggerOption == nullptr) + { + yError() << "[WalkingModule::configure] Unable to get the group LOGGER."; + return false; + } + + std::string logPort; + if (!loggerOption->getParameter("remote", logPort)) + { + yError() << m_logPrefix << "Unable to get the remote from the group LOGGER."; + return false; + } + + // prepend the module name to the port name + logPort = "/" + getName() + logPort; + loggerOption->setParameter("remote", logPort); + if (!m_vectorsCollectionServer.initialize(loggerOption)) + { + yError() << m_logPrefix << "Unable to get the string from searchable."; + return false; + } + } + // initialize the left hand teleoperation if (m_useLeftHand) { yarp::os::Bottle& leftFingersOptions = rf.findGroup("LEFT_FINGERS_RETARGETING"); leftFingersOptions.append(generalOptions); + if (m_enableLogger) + { + yarp::os::Bottle& loggerOptions = rf.findGroup("LOGGER"); + leftFingersOptions.append(loggerOptions); + } m_leftHand = std::make_unique(); - if (!m_leftHand->configure(leftFingersOptions, m_robot, false)) + if (!m_leftHand->configure(leftFingersOptions, m_robot, false, m_vectorsCollectionServer)) { yError() << m_logPrefix << "unable to initialize the left hand bilateral teleoperation."; @@ -71,14 +113,27 @@ bool HapticGloveModule::configure(yarp::os::ResourceFinder& rf) yarp::os::Bottle& rightFingersOptions = rf.findGroup("RIGHT_FINGERS_RETARGETING"); rightFingersOptions.append(generalOptions); + if (m_enableLogger) + { + yarp::os::Bottle& loggerOptions = rf.findGroup("LOGGER"); + rightFingersOptions.append(loggerOptions); + } + m_rightHand = std::make_unique(); - if (!m_rightHand->configure(rightFingersOptions, m_robot, true)) + if (!m_rightHand->configure(rightFingersOptions, m_robot, true, m_vectorsCollectionServer)) { yError() << m_logPrefix << "unable to initialize the right hand bilateral teleoperation."; return false; } } + + if (m_enableLogger) + { + // The two teleoperation objects fill the metadata internally + m_vectorsCollectionServer.finalizeMetadata(); + } + // wainting time after preparation and before running state machine m_waitingStartTime = 0; m_waitingDurationTime @@ -136,6 +191,12 @@ bool HapticGloveModule::updateModule() if (m_state == HapticGloveFSM::Running) { + if (m_enableLogger) + { + m_vectorsCollectionServer.prepareData(); + m_vectorsCollectionServer.clearData(); + } + if (m_useLeftHand) { if (!m_leftHand->run()) @@ -153,6 +214,11 @@ bool HapticGloveModule::updateModule() } } + if (m_enableLogger) + { + m_vectorsCollectionServer.sendData(); + } + } else if (m_state == HapticGloveFSM::Configuring) { // do nothing at the moment, it should be configured before diff --git a/modules/HapticGlove_module/src/Logger.cpp b/modules/HapticGlove_module/src/Logger.cpp index be0a3679..e6861016 100644 --- a/modules/HapticGlove_module/src/Logger.cpp +++ b/modules/HapticGlove_module/src/Logger.cpp @@ -9,415 +9,178 @@ #include using namespace HapticGlove; -Teleoperation::Logger::Logger(const Teleoperation& module, const bool isRightHand) - : m_teleoperation(module) +HapticGlove::Teleoperation::Logger::Logger( + const Teleoperation& module, + LoggerOptions options, + BipedalLocomotion::YarpUtilities::VectorsCollectionServer& loggerObject) + : m_handName(options.isRightHand ? "right_hand" : "left_hand") + , m_options(options) + , m_teleoperation(module) + , m_logger(loggerObject) { - - m_isRightHand = isRightHand; - m_handName = m_isRightHand ? "Right" : "Left"; - - m_robotPrefix = "robot" + m_handName + "Hand"; - m_humanPrefix = "human" + m_handName + "Hand"; - - m_logPrefix = "Logger::" + m_handName + ":: "; - - m_useSkin = m_teleoperation.m_useSkin; - - m_numRobotActuatedAxes - = m_teleoperation.m_robotController->controlHelper()->getNumberOfActuatedAxis(); - m_numRobotActuatedJoints - = m_teleoperation.m_robotController->controlHelper()->getNumberOfActuatedJoints(); - - m_numHumanHandFingers = m_teleoperation.m_humanGlove->getNumOfFingers(); - m_numHumanHandJoints = m_teleoperation.m_humanGlove->getNumOfHandJoints(); - m_numHumanVibrotactileFeedback = m_teleoperation.m_humanGlove->getNumOfVibrotactileFeedbacks(); - m_numHumanForceFeedback = m_teleoperation.m_humanGlove->getNumOfForceFeedback(); - if (m_useSkin) + // axis + m_logger.populateMetadata(m_handName + "::actuated_axes::measured", + m_teleoperation.getActuatedAxisNames()); + m_logger.populateMetadata(m_handName + "::actuated_axes::desired", + m_teleoperation.getActuatedAxisNames()); + m_logger.populateMetadata(m_handName + "::actuated_axes::pid_output", + m_teleoperation.getActuatedAxisNames()); + + m_logger.populateMetadata(m_handName + "::actuated_joints::measured", + m_teleoperation.getActuatedJointNames()); + m_logger.populateMetadata(m_handName + "::actuated_joints::desired", + m_teleoperation.getActuatedJointNames()); + + if (m_options.dumpKalmanFilterData) { - m_numberRobotTactileFeedbacks = m_teleoperation.m_robotSkin->getNumOfTactileFeedbacks(); + // axis reference KF + m_logger.populateMetadata(m_handName + + "::kalman_filter::actuated_axes::position::desired", + m_teleoperation.getActuatedAxisNames()); + m_logger.populateMetadata(m_handName + "::kalman_filter::actuated_axes::velocity::desired", + m_teleoperation.getActuatedAxisNames()); + m_logger.populateMetadata(m_handName + "::kalman_filter::actuated_axes::acceleration::desired", + m_teleoperation.getActuatedAxisNames()); + + // axis feedback KF + m_logger.populateMetadata(m_handName + "::kalman_filter::actuated_axes::position::measured", + m_teleoperation.getActuatedAxisNames()); + m_logger.populateMetadata(m_handName + "::kalman_filter::actuated_axes::velocity::measured", + m_teleoperation.getActuatedAxisNames()); + m_logger.populateMetadata(m_handName + "::kalman_filter::actuated_axes::acceleration::measured", + m_teleoperation.getActuatedAxisNames()); + + // joints KF + m_logger.populateMetadata(m_handName + "::kalman_filter::actuated_joints::position::measured", + m_teleoperation.getActuatedJointNames()); + m_logger.populateMetadata(m_handName + "::kalman_filter::actuated_joints::position::desired", + m_teleoperation.getActuatedJointNames()); } - // initialize the data structure - m_data.time = yarp::os::Time::now(); - // robot - m_data.robotAxisReferences.resize(m_numRobotActuatedAxes, 0.0); - m_data.robotAxisFeedbacks.resize(m_numRobotActuatedAxes, 0.0); - m_data.robotAxisVelocityFeedbacks.resize(m_numRobotActuatedAxes, 0.0); - - m_data.robotJointReferences.resize(m_numRobotActuatedJoints, 0.0); - m_data.robotJointFeedbacks.resize(m_numRobotActuatedJoints, 0.0); - - m_data.robotAxisValueErrors.resize(m_numRobotActuatedAxes, 0.0); - m_data.robotAxisVelocityErrors.resize(m_numRobotActuatedAxes, 0.0); - - m_data.robotMotorCurrentReferences.resize(m_numRobotActuatedAxes, 0.0); - m_data.robotMotorCurrentFeedbacks.resize(m_numRobotActuatedAxes, 0.0); - - m_data.robotMotorPwmReferences.resize(m_numRobotActuatedAxes, 0.0); - m_data.robotMotorPwmFeedbacks.resize(m_numRobotActuatedAxes, 0.0); - - m_data.robotMotorPidOutputs.resize(m_numRobotActuatedAxes, 0.0); - - m_data.robotAxisValueReferencesKf.resize(m_numRobotActuatedAxes, 0.0); - m_data.robotAxisVelocityReferencesKf.resize(m_numRobotActuatedAxes, 0.0); - m_data.robotAxisAccelerationReferencesKf.resize(m_numRobotActuatedAxes, 0.0); - m_data.robotAxisCovReferencesKf = Eigen::MatrixXd::Zero(m_numRobotActuatedAxes, 9); - - m_data.robotAxisValueFeedbacksKf.resize(m_numRobotActuatedAxes, 0.0); - m_data.robotAxisVelocityFeedbacksKf.resize(m_numRobotActuatedAxes, 0.0); - m_data.robotAxisAccelerationFeedbacksKf.resize(m_numRobotActuatedAxes, 0.0); - m_data.robotAxisCovFeedbacksKf = Eigen::MatrixXd::Zero(m_numRobotActuatedAxes, 9); - - m_data.robotJointsExpectedKf.resize(m_numRobotActuatedJoints, 0.0); - m_data.robotJointsFeedbackKf.resize(m_numRobotActuatedJoints, 0.0); - // human - m_data.humanJointValues.resize(m_numHumanHandJoints, 0.0); - m_data.humanFingertipPoses = Eigen::MatrixXd::Zero(m_numHumanHandFingers, 7); - m_data.humanForceFeedbacks.resize(m_numHumanForceFeedback, 0.0); - m_data.humanVibrotactileFeedbacks.resize(m_numHumanVibrotactileFeedback, 0.0); - m_data.humanPalmRotation.resize(4, 0.0); // 4: number of quaternions - - // skin data - if (m_useSkin) + if (m_options.dumpHumanData) { - m_data.fingertipsSkinData.resize(m_numberRobotTactileFeedbacks, 0.0); - m_data.fingertipsCalibratedTactileFeedback.resize(m_numberRobotTactileFeedbacks, 0.0); - m_data.fingertipsCalibratedDerivativeTactileFeedback.resize(m_numberRobotTactileFeedbacks, - 0.0); - m_data.fingercontactStrengthFeedback.resize(m_numHumanVibrotactileFeedback, 0.0); - m_data.fingercontactStrengthDerivativeFeedback.resize(m_numHumanVibrotactileFeedback, 0.0); - m_data.robotFingerSkinAbsoluteValueVibrotactileFeedbacks.resize( - m_numHumanVibrotactileFeedback, 0.0); - m_data.robotFingerSkinDerivativeValueVibrotactileFeedbacks.resize( - m_numHumanVibrotactileFeedback, 0.0); - m_data.robotFingerSkinTotalValueVibrotactileFeedbacks.resize(m_numHumanVibrotactileFeedback, - 0.0); - m_data.areFingersSkinInContact.resize(m_numHumanVibrotactileFeedback, 0.0); + // Human data + m_logger.populateMetadata(m_handName + "::human::joint_values", + m_teleoperation.getHumanHandJointsNames()); + m_logger.populateMetadata(m_handName + "::human::force_feedbacks", + m_teleoperation.getHumanHandFingerNames()); + m_logger.populateMetadata(m_handName + "::human::vibrotactile_feedbacks", + m_teleoperation.getHumanHandFingerNames()); } -} - -Teleoperation::Logger::~Logger() = default; - -bool Teleoperation::Logger::openLogger() -{ - -#ifdef ENABLE_LOGGER - std::string currentTime = YarpHelper::getTimeDateMatExtension(); - m_logFileName = "HapticGloveModule_" + m_handName + "Hand_" + currentTime + "_log.mat"; - - yInfo() << "log file name: " << currentTime << m_logFileName; - - m_logger = XBot::MatLogger2::MakeLogger(m_logFileName); - m_appender = XBot::MatAppender::MakeInstance(); - m_appender->add_logger(m_logger); - m_appender->start_flush_thread(); - - // create the data structures to save - // time - m_logger->create("time", 1); - - // axis - m_logger->create(m_robotPrefix + "AxisReferences", m_numRobotActuatedAxes); - m_logger->create(m_robotPrefix + "AxisFeedbacks", m_numRobotActuatedAxes); - m_logger->create(m_robotPrefix + "AxisVelocityFeedbacks", m_numRobotActuatedAxes); - - // robot hand joints - m_logger->create(m_robotPrefix + "JointReferences", m_numRobotActuatedJoints); - m_logger->create(m_robotPrefix + "JointFeedbacks", m_numRobotActuatedJoints); - - // robot axis errors - m_logger->create(m_robotPrefix + "AxisValueErrors", m_numRobotActuatedAxes); - m_logger->create(m_robotPrefix + "AxisVelocityErrors", m_numRobotActuatedAxes); - - // to check if it is real robot or simulation - if (m_teleoperation.m_robot == "icub") - { - // current - m_logger->create(m_robotPrefix + "MotorCurrentReferences", m_numRobotActuatedAxes); - m_logger->create(m_robotPrefix + "MotorCurrentFeedbacks", m_numRobotActuatedAxes); - - // pwm - m_logger->create(m_robotPrefix + "MotorPwmReferences", m_numRobotActuatedAxes); - m_logger->create(m_robotPrefix + "MotorPwmFeedbacks", m_numRobotActuatedAxes); - } - - // pid - m_logger->create(m_robotPrefix + "MotorPidOutputs", m_numRobotActuatedAxes); - - // axis reference KF - m_logger->create(m_robotPrefix + "AxisValueReferencesKf", m_numRobotActuatedAxes); - m_logger->create(m_robotPrefix + "AxisVelocityReferencesKf", m_numRobotActuatedAxes); - m_logger->create(m_robotPrefix + "AxisAccelerationReferencesKf", m_numRobotActuatedAxes); - m_logger->create(m_robotPrefix + "AxisCovReferencesKf", - m_numRobotActuatedAxes, - 9); // states: value, velocity, acceleration --> cov matrix size: 3X3=9 - - // axis feedback KF - m_logger->create(m_robotPrefix + "AxisValueFeedbacksKf", m_numRobotActuatedAxes); - m_logger->create(m_robotPrefix + "AxisVelocityFeedbacksKf", m_numRobotActuatedAxes); - m_logger->create(m_robotPrefix + "AxisAccelerationFeedbacksKf", m_numRobotActuatedAxes); - m_logger->create(m_robotPrefix + "AxisCovFeedbacksKf", - m_numRobotActuatedAxes, - 9); // states: value, velocity, acceleration --> cov matrix size: 3X3=9 - - // joints KF - m_logger->create(m_robotPrefix + "JointsExpectedKf", m_numRobotActuatedJoints); - m_logger->create(m_robotPrefix + "JointsFeedbackKf", m_numRobotActuatedJoints); - - // Human data - m_logger->create(m_humanPrefix + "JointValues", m_numHumanHandJoints); - m_logger->create(m_humanPrefix + "FingertipPoses", m_numHumanHandFingers, 7); - m_logger->create(m_humanPrefix + "ForceFeedbacks", m_numHumanForceFeedback); - m_logger->create(m_humanPrefix + "VibrotactileFeedbacks", m_numHumanVibrotactileFeedback); - m_logger->create(m_humanPrefix + "PalmRotation", 4); - - // add the robot and human fingers, axes, joints. - std::vector robotActuatedAxisNames; - std::vector robotActuatedJointNames; - std::vector humanJointNames; - std::vector humanFingerNames; - - m_teleoperation.m_robotController->controlHelper()->getActuatedAxisNames( - robotActuatedAxisNames); - m_teleoperation.m_robotController->controlHelper()->getActuatedJointNames( - robotActuatedJointNames); - m_teleoperation.m_humanGlove->getHumanHandJointsNames(humanJointNames); - m_teleoperation.m_humanGlove->getHumanHandFingerNames(humanFingerNames); - - m_logger->create(m_robotPrefix + "ActuatedAxisNames", m_numRobotActuatedAxes); - m_logger->create(m_robotPrefix + "ActuatedJointNames", m_numRobotActuatedJoints); - m_logger->create(m_humanPrefix + "JointNames", m_numHumanHandJoints); - m_logger->create(m_humanPrefix + "FingerNames", m_numHumanHandFingers); // skin data - if (m_useSkin) + if (m_options.dumpSkinData) { - m_logger->create(m_robotPrefix + "SkinData", m_numberRobotTactileFeedbacks); - m_logger->create(m_robotPrefix + "CalibratedSkinData", m_numberRobotTactileFeedbacks); - m_logger->create(m_robotPrefix + "CalibratedSkinDataDerivative", - m_numberRobotTactileFeedbacks); - m_logger->create(m_robotPrefix + "FingercontactStrength", m_numHumanVibrotactileFeedback); - m_logger->create(m_robotPrefix + "FingercontactStrengthDerivative", - m_numHumanVibrotactileFeedback); - m_logger->create(m_robotPrefix + "SkinAbsoluteValueVibrotactileFeedback", - m_numHumanVibrotactileFeedback); - m_logger->create(m_robotPrefix + "SkinDerivativeValueVibrotactileFeedback", - m_numHumanVibrotactileFeedback); - m_logger->create(m_robotPrefix + "SkinTotalValueVibrotactileFeedback", - m_numHumanVibrotactileFeedback); - m_logger->create(m_robotPrefix + "SkinIsInContact", m_numHumanVibrotactileFeedback); + const std::vector& tactileData + = m_teleoperation.getFingersTactileData(); + + std::vector fingerNames; + for (const auto& data : tactileData) + { + std::vector taxelNames; + for (size_t i = data.indexStart; i <= data.indexEnd; i++) + { + taxelNames.push_back("taxel_" + std::to_string(i)); + } + + m_logger.populateMetadata(m_handName + "::skin::" + data.fingerName + "::taxels::raw", + taxelNames); + m_logger.populateMetadata(m_handName + "::skin::" + data.fingerName + "::taxels::calibrated", + taxelNames); + m_logger.populateMetadata(m_handName + "::skin::" + data.fingerName + "::taxels::calibrated_derivative", + taxelNames); + fingerNames.push_back(data.fingerName); + } + m_logger.populateMetadata(m_handName + "::skin::contact_strength::value", fingerNames); + m_logger.populateMetadata(m_handName + "::skin::contact_strength::derivative", fingerNames); + m_logger.populateMetadata(m_handName + "::skin::vibrotactile_feedback::absolute_value", fingerNames); + m_logger.populateMetadata(m_handName + "::skin::vibrotactile_feedback::derivative_value", fingerNames); + m_logger.populateMetadata(m_handName + "::skin::vibrotactile_feedback::total_value", fingerNames); + m_logger.populateMetadata(m_handName + "::skin::is_in_contact", fingerNames); + m_fingerSkinContactBuffer.resize(fingerNames.size()); } - - // m_logger->add(m_robotPrefix + "ActuatedAxisNames", robotActuatedAxisNames); - // m_logger->add(m_robotPrefix + "ActuatedJointNames", robotActuatedJointNames); - // m_logger->add(m_humanPrefix + "JointNames", humanJointNames); - // m_logger->add(m_humanPrefix + "FingerNames", humanFingerNames); - - // print - yInfo() << m_logPrefix << "logging is active."; - -#else - yInfo() << m_logPrefix << " logging option is not active in CMakeLists."; - -#endif - - return true; } -bool Teleoperation::Logger::updateData() -{ - m_data.time = yarp::os::Time::now(); - - // robot - m_teleoperation.m_robotController->getAxisValueReferences(m_data.robotAxisReferences); - - m_teleoperation.m_robotController->getAxisValueFeedbacks(m_data.robotAxisFeedbacks); - - m_teleoperation.m_robotController->getAxisVelocityFeedbacks(m_data.robotAxisVelocityFeedbacks); - - m_teleoperation.m_robotController->getJointReferences(m_data.robotJointReferences); - - m_teleoperation.m_robotController->getJointValueFeedbacks(m_data.robotJointFeedbacks); - - m_teleoperation.m_retargeting->getAxisError(m_data.robotAxisValueErrors, - m_data.robotAxisVelocityErrors); - - if (m_teleoperation.m_robot == "icub") - { - // current - m_teleoperation.m_robotController->getMotorCurrentReference( - m_data.robotMotorCurrentReferences); - - m_teleoperation.m_robotController->getMotorCurrentFeedback( - m_data.robotMotorCurrentFeedbacks); - - // pwm - m_teleoperation.m_robotController->getMotorPwmReference(m_data.robotMotorPwmReferences); - - m_teleoperation.m_robotController->getMotorPwmFeedback(m_data.robotMotorPwmFeedbacks); - } - - m_teleoperation.m_robotController->getMotorPidOutputs(m_data.robotMotorPidOutputs); - - m_teleoperation.m_robotController->getEstimatedMotorsState( - m_data.robotAxisValueFeedbacksKf, - m_data.robotAxisVelocityFeedbacksKf, - m_data.robotAxisAccelerationFeedbacksKf, - m_data.robotAxisCovFeedbacksKf, - m_data.robotAxisValueReferencesKf, - m_data.robotAxisVelocityReferencesKf, - m_data.robotAxisAccelerationReferencesKf, - m_data.robotAxisCovReferencesKf); - - m_teleoperation.m_robotController->getEstimatedJointValuesKf(m_data.robotJointsExpectedKf, - m_data.robotJointsFeedbackKf); - // human - m_teleoperation.m_humanGlove->getHandJointAngles(m_data.humanJointValues); - - m_teleoperation.m_humanGlove->getFingertipPoses(m_data.humanFingertipPoses); - - m_teleoperation.m_retargeting->getForceFeedbackToHuman(m_data.humanForceFeedbacks); - - m_teleoperation.m_retargeting->getVibrotactileFeedbackToHuman( - m_data.humanVibrotactileFeedbacks); - - m_teleoperation.m_humanGlove->getHandPalmRotation(m_data.humanPalmRotation); - - // skin - if (m_useSkin) - { - m_teleoperation.m_robotSkin->getSerializedFingertipsTactileFeedbacks( - m_data.fingertipsSkinData); - - m_teleoperation.m_robotSkin->getSerializedFingertipsCalibratedTactileFeedbacks( - m_data.fingertipsCalibratedTactileFeedback); - - m_teleoperation.m_robotSkin->getSerializedFingertipsCalibratedTactileDerivativeFeedbacks( - m_data.fingertipsCalibratedDerivativeTactileFeedback); - - m_teleoperation.m_robotSkin->getFingertipsContactStrength( - m_data.fingercontactStrengthFeedback); - - m_teleoperation.m_robotSkin->getFingertipsContactStrengthDerivative( - m_data.fingercontactStrengthDerivativeFeedback); - - m_teleoperation.m_robotSkin->getVibrotactileAbsoluteFeedback( - m_data.robotFingerSkinAbsoluteValueVibrotactileFeedbacks); - - m_teleoperation.m_robotSkin->getVibrotactileDerivativeFeedback( - m_data.robotFingerSkinDerivativeValueVibrotactileFeedbacks); - - m_teleoperation.m_robotSkin->getVibrotactileTotalFeedback( - m_data.robotFingerSkinTotalValueVibrotactileFeedbacks); - - m_teleoperation.m_robotSkin->areFingersInContact(m_data.areFingersSkinInContact); - } - - return true; -} bool Teleoperation::Logger::logData() { -#ifdef ENABLE_LOGGER + const auto& data = m_teleoperation.getData(); + m_logger.populateData(m_handName + "::actuated_axes::measured", data.robotAxisFeedbacks); + m_logger.populateData(m_handName + "::actuated_axes::desired", data.robotAxisReferences); + m_logger.populateData(m_handName + "::actuated_axes::pid_output", data.robotMotorPidOutputs); + + m_logger.populateData(m_handName + "::actuated_joints::measured", data.robotJointFeedbacks); + m_logger.populateData(m_handName + "::actuated_joints::desired",data.robotJointReferences); - if (!this->updateData()) + if (m_options.dumpKalmanFilterData) { - yWarning() << m_logPrefix << "cannot update the data."; + // axis reference KF + m_logger.populateData(m_handName + "::kalman_filter::actuated_axes::position::desired", + data.robotAxisValueReferencesKf); + m_logger.populateData(m_handName + "::kalman_filter::actuated_axes::velocity::desired", + data.robotAxisVelocityReferencesKf); + m_logger.populateData(m_handName + "::kalman_filter::actuated_axes::acceleration::desired", + data.robotAxisAccelerationReferencesKf); + + // axis feedback KF + m_logger.populateData(m_handName + "::kalman_filter::actuated_axes::position::measured", + data.robotAxisValueFeedbacksKf); + m_logger.populateData(m_handName + "::kalman_filter::actuated_axes::velocity::measured", + data.robotAxisVelocityFeedbacksKf); + m_logger.populateData(m_handName + "::kalman_filter::actuated_axes::acceleration::measured", + data.robotAxisAccelerationFeedbacksKf); + + // joints KF + m_logger.populateData(m_handName + "::kalman_filter::actuated_joints::position::measured", + data.robotJointsFeedbackKf); + m_logger.populateData(m_handName + "::kalman_filter::actuated_joints::position::desired", + data.robotJointsExpectedKf); } - // time - m_logger->add("time", m_data.time); - - // axis - m_logger->add(m_robotPrefix + "AxisReferences", m_data.robotAxisReferences); - m_logger->add(m_robotPrefix + "AxisFeedbacks", m_data.robotAxisFeedbacks); - m_logger->add(m_robotPrefix + "AxisVelocityFeedbacks", m_data.robotAxisVelocityFeedbacks); - - // robot hand joints - m_logger->add(m_robotPrefix + "JointReferences", m_data.robotJointReferences); - m_logger->add(m_robotPrefix + "JointFeedbacks", m_data.robotJointFeedbacks); - - // robot axis errors - m_logger->add(m_robotPrefix + "AxisValueErrors", m_data.robotAxisValueErrors); - m_logger->add(m_robotPrefix + "AxisVelocityErrors", m_data.robotAxisVelocityErrors); - - // to check if it is real robot or simulation - if (m_teleoperation.m_robot == "icub") + if (m_options.dumpHumanData) { - // current - m_logger->add(m_robotPrefix + "MotorCurrentReferences", m_data.robotMotorCurrentReferences); - m_logger->add(m_robotPrefix + "MotorCurrentFeedbacks", m_data.robotMotorCurrentFeedbacks); - - // pwm - m_logger->add(m_robotPrefix + "MotorPwmReferences", m_data.robotMotorPwmReferences); - m_logger->add(m_robotPrefix + "MotorPwmFeedbacks", m_data.robotMotorPwmFeedbacks); + // Human data + m_logger.populateData(m_handName + "::human::joint_values", + data.humanJointValues); + m_logger.populateData(m_handName + "::human::force_feedbacks", + data.humanForceFeedbacks); + m_logger.populateData(m_handName + "::human::vibrotactile_feedbacks", + data.humanVibrotactileFeedbacks); } - // pid - m_logger->add(m_robotPrefix + "MotorPidOutputs", m_data.robotMotorPidOutputs); - - // axis reference KF - m_logger->add(m_robotPrefix + "AxisValueReferencesKf", m_data.robotAxisValueReferencesKf); - m_logger->add(m_robotPrefix + "AxisVelocityReferencesKf", m_data.robotAxisVelocityReferencesKf); - m_logger->add(m_robotPrefix + "AxisAccelerationReferencesKf", - m_data.robotAxisAccelerationReferencesKf); - m_logger->add(m_robotPrefix + "AxisCovReferencesKf", m_data.robotAxisCovReferencesKf); - - // axis feedback KF - m_logger->add(m_robotPrefix + "AxisValueFeedbacksKf", m_data.robotAxisValueFeedbacksKf); - m_logger->add(m_robotPrefix + "AxisVelocityFeedbacksKf", m_data.robotAxisVelocityFeedbacksKf); - m_logger->add(m_robotPrefix + "AxisAccelerationFeedbacksKf", - m_data.robotAxisAccelerationFeedbacksKf); - m_logger->add(m_robotPrefix + "AxisCovFeedbacksKf", m_data.robotAxisCovFeedbacksKf); - - // joints KF - m_logger->add(m_robotPrefix + "JointsExpectedKf", m_data.robotJointsExpectedKf); - m_logger->add(m_robotPrefix + "JointsFeedbackKf", m_data.robotJointsFeedbackKf); - - // Human data - m_logger->add(m_humanPrefix + "JointValues", m_data.humanJointValues); - m_logger->add(m_humanPrefix + "FingertipPoses", m_data.humanFingertipPoses); - m_logger->add(m_humanPrefix + "ForceFeedbacks", m_data.humanForceFeedbacks); - m_logger->add(m_humanPrefix + "VibrotactileFeedbacks", m_data.humanVibrotactileFeedbacks); - m_logger->add(m_humanPrefix + "PalmRotation", m_data.humanPalmRotation); - - // skin - if (m_useSkin) + // skin data + if (m_options.dumpSkinData) { - m_logger->add(m_robotPrefix + "SkinData", m_data.fingertipsSkinData); - m_logger->add(m_robotPrefix + "CalibratedSkinData", - m_data.fingertipsCalibratedTactileFeedback); - m_logger->add(m_robotPrefix + "CalibratedSkinDataDerivative", - m_data.fingertipsCalibratedDerivativeTactileFeedback); - m_logger->add(m_robotPrefix + "FingercontactStrength", - m_data.fingercontactStrengthFeedback); - m_logger->add(m_robotPrefix + "FingercontactStrengthDerivative", - m_data.fingercontactStrengthDerivativeFeedback); - m_logger->add(m_robotPrefix + "SkinAbsoluteValueVibrotactileFeedback", - m_data.robotFingerSkinAbsoluteValueVibrotactileFeedbacks); - m_logger->add(m_robotPrefix + "SkinDerivativeValueVibrotactileFeedback", - m_data.robotFingerSkinDerivativeValueVibrotactileFeedbacks); - m_logger->add(m_robotPrefix + "SkinTotalValueVibrotactileFeedback", - m_data.robotFingerSkinTotalValueVibrotactileFeedbacks); - - std::vector areFingersSkinInContact(m_data.areFingersSkinInContact.begin(), - m_data.areFingersSkinInContact.end()); - m_logger->add(m_robotPrefix + "SkinIsInContact", areFingersSkinInContact); + const std::vector& tactileData + = m_teleoperation.getFingersTactileData(); + + for (const auto& finger : tactileData) + { + m_logger.populateData(m_handName + "::skin::" + finger.fingerName + "::taxels::raw", + iDynTree::Span(data.fingertipsSkinData.data() + finger.indexStart, finger.noTactileSensors)); + m_logger.populateData(m_handName + "::skin::" + finger.fingerName + "::taxels::calibrated", + iDynTree::Span(data.fingertipsCalibratedTactileFeedback.data() + finger.indexStart, + finger.noTactileSensors)); + m_logger.populateData(m_handName + "::skin::" + finger.fingerName + "::taxels::calibrated_derivative", + iDynTree::Span(data.fingertipsCalibratedDerivativeTactileFeedback.data() + + finger.indexStart, + finger.noTactileSensors)); + } + m_logger.populateData(m_handName + "::skin::contact_strength::value", + data.fingercontactStrengthFeedback); + m_logger.populateData(m_handName + "::skin::contact_strength::derivative", + data.fingercontactStrengthDerivativeFeedback); + m_logger.populateData(m_handName + "::skin::vibrotactile_feedback::absolute_value", + data.robotFingerSkinAbsoluteValueVibrotactileFeedbacks); + m_logger.populateData(m_handName + "::skin::vibrotactile_feedback::derivative_value", + data.robotFingerSkinDerivativeValueVibrotactileFeedbacks); + m_logger.populateData(m_handName + "::skin::vibrotactile_feedback::total_value", + data.robotFingerSkinTotalValueVibrotactileFeedbacks); + for (size_t i = 0; i < data.areFingersSkinInContact.size(); i++) + { + m_fingerSkinContactBuffer[i] = data.areFingersSkinInContact[i] ? 1.0 : 0.0; + } + m_logger.populateData(m_handName + "::skin::is_in_contact", m_fingerSkinContactBuffer); } -#endif - - return true; -} - -bool Teleoperation::Logger::closeLogger() -{ - -#ifdef ENABLE_LOGGER - m_logger->flush_available_data(); - // m_logger->~MatLogger2(); - m_logger = nullptr; -#endif - yInfo() << m_logPrefix << "logger is closing."; - yInfo() << m_logPrefix << "log file is saved in: " << m_logFileName; return true; } diff --git a/modules/HapticGlove_module/src/Teleoperation.cpp b/modules/HapticGlove_module/src/Teleoperation.cpp index e5083ff7..df6fea6e 100644 --- a/modules/HapticGlove_module/src/Teleoperation.cpp +++ b/modules/HapticGlove_module/src/Teleoperation.cpp @@ -23,7 +23,8 @@ Teleoperation::~Teleoperation() = default; bool Teleoperation::configure(const yarp::os::Searchable& config, const std::string& name, - const bool& rightHand) + const bool& rightHand, + BipedalLocomotion::YarpUtilities::VectorsCollectionServer& loggerObject) { m_logPrefix += rightHand ? "RightHand:: " : "LeftHand:: "; @@ -133,6 +134,7 @@ bool Teleoperation::configure(const yarp::os::Searchable& config, m_data.robotAxisReferences.resize(numRobotActuatedAxis, 0.0); m_data.robotAxisFeedbacks.resize(numRobotActuatedAxis, 0.0); m_data.robotAxisVelocityFeedbacks.resize(numRobotActuatedAxis, 0.0); + m_data.robotMotorPidOutputs.resize(numRobotActuatedAxis, 0.0); m_data.robotJointReferences.resize(numRobotActuatedJoints, 0.0); m_data.robotJointFeedbacks.resize(numRobotActuatedJoints, 0.0); @@ -149,6 +151,8 @@ bool Teleoperation::configure(const yarp::os::Searchable& config, m_data.robotAxisVelocityReferencesKf.resize(numRobotActuatedAxis, 0.0); m_data.robotAxisAccelerationReferencesKf.resize(numRobotActuatedAxis, 0.0); m_data.robotAxisCovReferencesKf = Eigen::MatrixXd::Zero(numRobotActuatedAxis, 9); + m_data.robotJointsExpectedKf.resize(numRobotActuatedAxis, 0.0); + m_data.robotJointsFeedbackKf.resize(numRobotActuatedAxis, 0.0); // human m_data.humanJointValues.resize(numHumanHandJoints, 0.0); @@ -162,6 +166,17 @@ bool Teleoperation::configure(const yarp::os::Searchable& config, m_data.robotFingerSkinDerivativeValueVibrotactileFeedbacks.resize(numRobotFingers, 0.0); m_data.robotFingerSkinTotalValueVibrotactileFeedbacks.resize(numRobotFingers, 0.0); + if (m_useSkin) + { + //initialize some more data for walking + size_t numberRobotTactileFeedbacks = m_robotSkin->getNumOfTactileFeedbacks(); + m_data.fingertipsSkinData.resize(numberRobotTactileFeedbacks, 0.0); + m_data.fingertipsCalibratedTactileFeedback.resize(numberRobotTactileFeedbacks, 0.0); + m_data.fingertipsCalibratedDerivativeTactileFeedback.resize(numberRobotTactileFeedbacks, 0.0); + m_data.fingercontactStrengthFeedback.resize(numberRobotTactileFeedbacks, 0.0); + m_data.fingercontactStrengthDerivativeFeedback.resize(numberRobotTactileFeedbacks, 0.0); + } + // set up the glove if (!m_humanGlove->setupGlove()) { @@ -174,12 +189,14 @@ bool Teleoperation::configure(const yarp::os::Searchable& config, if (m_enableLogger) { - m_loggerLeftHand = std::make_unique(*this, rightHand); - if (!m_loggerLeftHand->openLogger()) - { - yError() << m_logPrefix << "unable to open the logger."; - return false; - } + LoggerOptions loggerOptions; + loggerOptions.isRightHand = rightHand; + loggerOptions.dumpSkinData = m_useSkin; + loggerOptions.dumpHumanData = config.check("logHumanData", yarp::os::Value(false)).asBool(); + loggerOptions.dumpKalmanFilterData + = config.check("logKalmanFilterData", yarp::os::Value(false)).asBool(); + + m_logger = std::make_unique(*this, loggerOptions, loggerObject); } // Initialize RPC @@ -332,6 +349,16 @@ bool Teleoperation::run() m_data.doRobotFingerSkinsWork, m_data.areFingersSkinInContact, m_data.robotFingerSkinTotalValueVibrotactileFeedbacks); + + // get some more data for logging + m_robotSkin->getSerializedFingertipsTactileFeedbacks(m_data.fingertipsSkinData); + m_robotSkin->getSerializedFingertipsCalibratedTactileFeedbacks( + m_data.fingertipsCalibratedTactileFeedback); + m_robotSkin->getSerializedFingertipsCalibratedTactileDerivativeFeedbacks( + m_data.fingertipsCalibratedDerivativeTactileFeedback); + m_robotSkin->getFingertipsContactStrength(m_data.fingercontactStrengthFeedback); + m_robotSkin->getFingertipsContactStrengthDerivative( + m_data.fingercontactStrengthDerivativeFeedback); } if (!m_retargeting->getForceFeedbackToHuman(m_data.humanForceFeedbacks)) @@ -353,9 +380,14 @@ bool Teleoperation::run() m_humanGlove->sendFingertipHapticFeedbackReferences(); } + //Get some data needed for logging + m_robotController->getMotorPidOutputs(m_data.robotMotorPidOutputs); + m_robotController->getEstimatedJointValuesKf(m_data.robotJointsExpectedKf, + m_data.robotJointsFeedbackKf); + if (m_enableLogger) { - if (!m_loggerLeftHand->logData()) + if (!m_logger->logData()) { yWarning() << m_logPrefix << "unable to log the data."; } @@ -471,14 +503,6 @@ bool Teleoperation::close() yInfo() << m_logPrefix << "trying to close."; bool ok = true; - if (m_enableLogger) - { - if (!m_loggerLeftHand->closeLogger()) - { - yWarning() << m_logPrefix << "unable to close the logger."; - ok &= false; - } - } if (!m_humanGlove->stopHapticFeedback()) { yWarning() << m_logPrefix << "cannot stop haptic feedback."; From 18606f6ddff030c07fc3076e6a68081988b4484e Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 1 Aug 2024 10:00:11 +0200 Subject: [PATCH 11/11] Updated haptic glove configuration files with new logger options --- app/robots/ergoCubGazeboV1/hapticGloveConfig.ini | 7 ++++++- app/robots/ergoCubSN000/hapticGloveConfig.ini | 7 ++++++- app/robots/ergoCubSN001/hapticGloveConfig.ini | 7 ++++++- app/robots/ergoCubSN002/hapticGloveConfig.ini | 7 ++++++- app/robots/iCubErzelli02/hapticGloveConfig.ini | 4 ++++ app/robots/iCubGazeboV2_5/hapticGloveConfig.ini | 4 ++++ app/robots/iCubGazeboV3/hapticGloveConfig.ini | 5 +++++ app/robots/iCubGenova04/hapticGloveConfig.ini | 5 +++++ app/robots/iCubGenova09/hapticGloveConfig.ini | 7 ++++++- app/robots/icubGazeboSim/hapticGloveConfig.ini | 5 +++++ 10 files changed, 53 insertions(+), 5 deletions(-) diff --git a/app/robots/ergoCubGazeboV1/hapticGloveConfig.ini b/app/robots/ergoCubGazeboV1/hapticGloveConfig.ini index 67b1b48d..edf73370 100644 --- a/app/robots/ergoCubGazeboV1/hapticGloveConfig.ini +++ b/app/robots/ergoCubGazeboV1/hapticGloveConfig.ini @@ -5,7 +5,7 @@ name HapticGloveRetargeting robot ergocubSim samplingTime 0.01 enableMoveRobot 1 -enableLogger 0 +enableLogger 1 useLeftHand 1 useRightHand 1 isMandatory 0 @@ -26,6 +26,11 @@ tactileWorkingThreshold 0.0001 # threshold in which an skin data is considered updated [no units] tactileUpdateThreshold -1.0 +[LOGGER] +remote "/logger" +logHumanData true +logKalmanFilterData false + # include fingers parameters [include LEFT_FINGERS_RETARGETING "leftFingersHapticRetargetingParams.ini"] [include RIGHT_FINGERS_RETARGETING "rightFingersHapticRetargetingParams.ini"] diff --git a/app/robots/ergoCubSN000/hapticGloveConfig.ini b/app/robots/ergoCubSN000/hapticGloveConfig.ini index e13f056a..5fbf25b9 100644 --- a/app/robots/ergoCubSN000/hapticGloveConfig.ini +++ b/app/robots/ergoCubSN000/hapticGloveConfig.ini @@ -5,7 +5,7 @@ name HapticGloveRetargeting robot ergocub samplingTime 0.1 enableMoveRobot 1 -enableLogger 0 +enableLogger 1 useLeftHand 1 useRightHand 1 isMandatory 0 @@ -26,6 +26,11 @@ tactileWorkingThreshold 0.0001 # threshold in which an skin data is considered updated [no units] tactileUpdateThreshold -1.0 +[LOGGER] +remote "/logger" +logHumanData true +logKalmanFilterData false + # include fingers parameters [include LEFT_FINGERS_RETARGETING "leftFingersHapticRetargetingParams.ini"] [include RIGHT_FINGERS_RETARGETING "rightFingersHapticRetargetingParams.ini"] diff --git a/app/robots/ergoCubSN001/hapticGloveConfig.ini b/app/robots/ergoCubSN001/hapticGloveConfig.ini index e13f056a..5fbf25b9 100644 --- a/app/robots/ergoCubSN001/hapticGloveConfig.ini +++ b/app/robots/ergoCubSN001/hapticGloveConfig.ini @@ -5,7 +5,7 @@ name HapticGloveRetargeting robot ergocub samplingTime 0.1 enableMoveRobot 1 -enableLogger 0 +enableLogger 1 useLeftHand 1 useRightHand 1 isMandatory 0 @@ -26,6 +26,11 @@ tactileWorkingThreshold 0.0001 # threshold in which an skin data is considered updated [no units] tactileUpdateThreshold -1.0 +[LOGGER] +remote "/logger" +logHumanData true +logKalmanFilterData false + # include fingers parameters [include LEFT_FINGERS_RETARGETING "leftFingersHapticRetargetingParams.ini"] [include RIGHT_FINGERS_RETARGETING "rightFingersHapticRetargetingParams.ini"] diff --git a/app/robots/ergoCubSN002/hapticGloveConfig.ini b/app/robots/ergoCubSN002/hapticGloveConfig.ini index e13f056a..5fbf25b9 100644 --- a/app/robots/ergoCubSN002/hapticGloveConfig.ini +++ b/app/robots/ergoCubSN002/hapticGloveConfig.ini @@ -5,7 +5,7 @@ name HapticGloveRetargeting robot ergocub samplingTime 0.1 enableMoveRobot 1 -enableLogger 0 +enableLogger 1 useLeftHand 1 useRightHand 1 isMandatory 0 @@ -26,6 +26,11 @@ tactileWorkingThreshold 0.0001 # threshold in which an skin data is considered updated [no units] tactileUpdateThreshold -1.0 +[LOGGER] +remote "/logger" +logHumanData true +logKalmanFilterData false + # include fingers parameters [include LEFT_FINGERS_RETARGETING "leftFingersHapticRetargetingParams.ini"] [include RIGHT_FINGERS_RETARGETING "rightFingersHapticRetargetingParams.ini"] diff --git a/app/robots/iCubErzelli02/hapticGloveConfig.ini b/app/robots/iCubErzelli02/hapticGloveConfig.ini index 1dea5da7..f4b80c62 100644 --- a/app/robots/iCubErzelli02/hapticGloveConfig.ini +++ b/app/robots/iCubErzelli02/hapticGloveConfig.ini @@ -23,6 +23,10 @@ tactileWorkingThreshold 0.0001 # threshold in which an skin data is considered updated [no units] tactileUpdateThreshold 0.0001 +[LOGGER] +remote "/logger" +logHumanData true +logKalmanFilterData false # include fingers parameters [include LEFT_FINGERS_RETARGETING "leftFingersHapticRetargetingParams.ini"] diff --git a/app/robots/iCubGazeboV2_5/hapticGloveConfig.ini b/app/robots/iCubGazeboV2_5/hapticGloveConfig.ini index b21e1fdc..c14aa708 100644 --- a/app/robots/iCubGazeboV2_5/hapticGloveConfig.ini +++ b/app/robots/iCubGazeboV2_5/hapticGloveConfig.ini @@ -26,6 +26,10 @@ tactileWorkingThreshold 0.0001 # threshold in which an skin data is considered updated [no units] tactileUpdateThreshold 0.0001 +[LOGGER] +remote "/logger" +logHumanData true +logKalmanFilterData false # include fingers parameters [include LEFT_FINGERS_RETARGETING "leftFingersHapticRetargetingParams.ini"] diff --git a/app/robots/iCubGazeboV3/hapticGloveConfig.ini b/app/robots/iCubGazeboV3/hapticGloveConfig.ini index cb174443..8cd02855 100644 --- a/app/robots/iCubGazeboV3/hapticGloveConfig.ini +++ b/app/robots/iCubGazeboV3/hapticGloveConfig.ini @@ -24,6 +24,11 @@ waitingDurationTime 5.0 # threshold in which an skin is considered as working fine [no units] tactileWorkingThreshold 0.0001 +[LOGGER] +remote "/logger" +logHumanData true +logKalmanFilterData false + # include fingers parameters [include LEFT_FINGERS_RETARGETING "leftFingersHapticRetargetingParams.ini"] [include RIGHT_FINGERS_RETARGETING "rightFingersHapticRetargetingParams.ini"] diff --git a/app/robots/iCubGenova04/hapticGloveConfig.ini b/app/robots/iCubGenova04/hapticGloveConfig.ini index ca6c09b9..24b53956 100644 --- a/app/robots/iCubGenova04/hapticGloveConfig.ini +++ b/app/robots/iCubGenova04/hapticGloveConfig.ini @@ -26,6 +26,11 @@ tactileWorkingThreshold 0.0001 # threshold in which an skin data is considered updated [no units] tactileUpdateThreshold 0.0001 +[LOGGER] +remote "/logger" +logHumanData true +logKalmanFilterData false + # include fingers parameters [include LEFT_FINGERS_RETARGETING "leftFingersHapticRetargetingParams.ini"] [include RIGHT_FINGERS_RETARGETING "rightFingersHapticRetargetingParams.ini"] diff --git a/app/robots/iCubGenova09/hapticGloveConfig.ini b/app/robots/iCubGenova09/hapticGloveConfig.ini index 09562a20..27f578c9 100644 --- a/app/robots/iCubGenova09/hapticGloveConfig.ini +++ b/app/robots/iCubGenova09/hapticGloveConfig.ini @@ -5,7 +5,7 @@ name HapticGloveRetargeting robot icub samplingTime 0.01 enableMoveRobot 1 -enableLogger 0 +enableLogger 1 useLeftHand 1 useRightHand 1 isMandatory 0 @@ -26,6 +26,11 @@ tactileWorkingThreshold 0.0001 # threshold in which an skin data is considered updated [no units] tactileUpdateThreshold -1.0 +[LOGGER] +remote "/logger" +logHumanData true +logKalmanFilterData false + # include fingers parameters [include LEFT_FINGERS_RETARGETING "leftFingersHapticRetargetingParams.ini"] [include RIGHT_FINGERS_RETARGETING "rightFingersHapticRetargetingParams.ini"] diff --git a/app/robots/icubGazeboSim/hapticGloveConfig.ini b/app/robots/icubGazeboSim/hapticGloveConfig.ini index cb174443..8cd02855 100644 --- a/app/robots/icubGazeboSim/hapticGloveConfig.ini +++ b/app/robots/icubGazeboSim/hapticGloveConfig.ini @@ -24,6 +24,11 @@ waitingDurationTime 5.0 # threshold in which an skin is considered as working fine [no units] tactileWorkingThreshold 0.0001 +[LOGGER] +remote "/logger" +logHumanData true +logKalmanFilterData false + # include fingers parameters [include LEFT_FINGERS_RETARGETING "leftFingersHapticRetargetingParams.ini"] [include RIGHT_FINGERS_RETARGETING "rightFingersHapticRetargetingParams.ini"]