├── libraries ├── CMakeLists.txt ├── wholeBodyDynamicsHelpers │ └── CMakeLists.txt └── ctrlLibRT │ ├── CMakeLists.txt │ ├── include │ └── ctrlLibRT │ │ └── filters.h │ └── src │ └── filters.cpp ├── scripts ├── CMakeLists.txt └── liftiCubInGui.sh ├── .gitattributes ├── doc ├── resources │ └── fbeV1.png └── howto │ └── force_control_on_icub.md ├── idl ├── VectorsCollection │ ├── VectorsCollection.thrift │ └── CMakeLists.txt ├── CMakeLists.txt ├── floatingBaseEstimatorRPC │ ├── CMakeLists.txt │ └── codyco │ │ └── floatingBaseEstimatorRPC.thrift ├── wholeBodyDynamicsSettings │ ├── CMakeLists.txt │ └── wholeBodyDynamicsSettings.thrift └── wholeBodyDynamics_IDLServer │ └── CMakeLists.txt ├── devices ├── baseEstimatorV1 │ ├── baseEstimatorV1.ini │ ├── include │ │ ├── WalkingLogger.tpp │ │ ├── WalkingLogger.hpp │ │ └── Utils.tpp │ ├── ros │ │ └── fbeViz.launch │ ├── app │ │ ├── CMakeLists.txt │ │ └── robots │ │ │ ├── iCubGenova04 │ │ │ ├── launch-fbe-analogsens.xml │ │ │ └── estimators │ │ │ │ └── fbe-analogsens.xml │ │ │ └── iCubGazeboV2_5 │ │ │ ├── launch-fbe-analogsens.xml │ │ │ └── estimators │ │ │ └── fbe-analogsens.xml │ ├── thrifts │ │ └── floatingBaseEstimationRPC.thrift │ ├── CMakeLists.txt │ ├── src │ │ └── WalkingLogger.cpp │ ├── scope │ │ ├── waist_imu_scope.xml │ │ ├── contact_scope.xml │ │ ├── base_velocity.xml │ │ └── base_scope.xml │ └── README.md ├── wholeBodyDynamics │ ├── wholebodydynamics.ini │ ├── test │ │ ├── CMakeLists.txt │ │ ├── fakeFTs │ │ │ ├── CMakeLists.txt │ │ │ ├── fakeFTs.h │ │ │ └── fakeFTs.cpp │ │ ├── ergocub_test_all.xml │ │ ├── WholeBodyDynamicsUnitTests.cpp │ │ └── ergocub_simulated_wholebodydynamics.xml │ ├── CMakeLists.txt │ ├── app │ │ ├── launch-wholebodydynamics-icub3-sim.xml │ │ ├── launch-wholebodydynamics-icub-six-fts-sim.xml │ │ ├── CMakeLists.txt │ │ ├── launch-wholebodydynamics-icub-four-fts.xml │ │ └── launch-wholebodydynamics-icub-six-fts.xml │ ├── SixAxisForceTorqueMeasureHelpers.cpp │ ├── SixAxisForceTorqueMeasureHelpers.h │ ├── GravityCompensationHelpers.h │ └── GravityCompensationHelpers.cpp ├── genericSensorClient │ ├── genericSensorClient.ini │ ├── CMakeLists.txt │ ├── GenericSensorClient.h │ └── GenericSensorClient.cpp ├── virtualAnalogClient │ ├── virtualAnalogClient.ini │ ├── CMakeLists.txt │ ├── VirtualAnalogClient.h │ └── VirtualAnalogClient.cpp ├── virtualAnalogRemapper │ ├── virtualAnalogRemapper.ini │ ├── CMakeLists.txt │ └── VirtualAnalogRemapper.h └── CMakeLists.txt ├── pixi_activation.sh ├── pixi_activation.bat ├── pixi_source_deps.yaml ├── .gitignore ├── pixi_source_deps_options.meta ├── cmake ├── WBEAddUnitTest.cmake ├── AddUninstallTarget.cmake └── AddInstallRPATHSupport.cmake ├── .github └── workflows │ ├── pixi-auto-update-ci.yml │ └── pixi-ci.yml ├── CMakeLists.txt ├── pixi.toml └── README.md /libraries/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(ctrlLibRT) 2 | -------------------------------------------------------------------------------- /scripts/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | yarp_install(PROGRAMS liftiCubInGui.sh DESTINATION bin) 2 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | # GitHub syntax highlighting 2 | pixi.lock linguist-language=YAML 3 | 4 | -------------------------------------------------------------------------------- /doc/resources/fbeV1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gbionics/whole-body-estimators/HEAD/doc/resources/fbeV1.png -------------------------------------------------------------------------------- /idl/VectorsCollection/VectorsCollection.thrift: -------------------------------------------------------------------------------- 1 | struct VectorsCollection 2 | { 3 | 1: map> vectors; 4 | } 5 | -------------------------------------------------------------------------------- /devices/baseEstimatorV1/baseEstimatorV1.ini: -------------------------------------------------------------------------------- 1 | [plugin baseEstimatorV1] 2 | type device 3 | name baseEstimatorV1 4 | library baseEstimatorV1 5 | -------------------------------------------------------------------------------- /devices/wholeBodyDynamics/wholebodydynamics.ini: -------------------------------------------------------------------------------- 1 | [plugin wholebodydynamics] 2 | type device 3 | name wholebodydynamics 4 | library wholeBodyDynamicsDevice 5 | -------------------------------------------------------------------------------- /scripts/liftiCubInGui.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "0.0 0.0 0.0 0.0 0.0 0.0" | yarp write ... /iCubGui/base:i 4 | 5 | echo "!!iCub lifted in the GUI!!" 6 | -------------------------------------------------------------------------------- /devices/genericSensorClient/genericSensorClient.ini: -------------------------------------------------------------------------------- 1 | [plugin genericSensorClient] 2 | type device 3 | name genericSensorClient 4 | library genericSensorClient 5 | -------------------------------------------------------------------------------- /devices/virtualAnalogClient/virtualAnalogClient.ini: -------------------------------------------------------------------------------- 1 | [plugin virtualAnalogClient] 2 | type device 3 | name virtualAnalogClient 4 | library virtualAnalogClient 5 | -------------------------------------------------------------------------------- /devices/virtualAnalogRemapper/virtualAnalogRemapper.ini: -------------------------------------------------------------------------------- 1 | [plugin virtualAnalogRemapper] 2 | type device 3 | name virtualAnalogRemapper 4 | library virtualAnalogRemapper 5 | -------------------------------------------------------------------------------- /idl/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(wholeBodyDynamicsSettings) 2 | add_subdirectory(wholeBodyDynamics_IDLServer) 3 | add_subdirectory(floatingBaseEstimatorRPC) 4 | add_subdirectory(VectorsCollection) 5 | -------------------------------------------------------------------------------- /pixi_activation.sh: -------------------------------------------------------------------------------- 1 | export CMAKE_INSTALL_PREFIX=$CONDA_PREFIX 2 | export CMAKE_PREFIX_PATH=$CMAKE_INSTALL_PREFIX:$CMAKE_PREFIX_PATH 3 | export PYTHONPATH= 4 | export YARP_DATA_DIRS=$CMAKE_INSTALL_PREFIX/share/yarp:$CMAKE_INSTALL_PREFIX/share/ergoCub 5 | -------------------------------------------------------------------------------- /pixi_activation.bat: -------------------------------------------------------------------------------- 1 | set CMAKE_INSTALL_PREFIX=%CONDA_PREFIX%\Library 2 | set CMAKE_PREFIX_PATH=%CMAKE_INSTALL_PREFIX%;%CMAKE_PREFIX_PATH% 3 | set PYTHONPATH="" 4 | set YARP_DATA_DIRS=%CMAKE_INSTALL_PREFIX%\share\yarp;%CMAKE_INSTALL_PREFIX%\share\ergoCub 5 | -------------------------------------------------------------------------------- /pixi_source_deps.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | YARP: 3 | type: git 4 | url: https://github.com/robotology/yarp.git 5 | version: e3ec363c0af7469fcb2c218c356844b8d6bbb1c5 6 | ICUB: 7 | type: git 8 | url: https://github.com/robotology/icub-main.git 9 | version: v2.5.0 10 | ergocub-software: 11 | type: git 12 | url: https://github.com/icub-tech-iit/ergocub-software.git 13 | version: v0.7.1 14 | -------------------------------------------------------------------------------- /devices/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. 2 | # This software may be modified and distributed under the terms of the 3 | # GNU Lesser General Public License v2.1 or any later version. 4 | 5 | add_subdirectory(baseEstimatorV1) 6 | add_subdirectory(wholeBodyDynamics) 7 | add_subdirectory(virtualAnalogClient) 8 | add_subdirectory(virtualAnalogRemapper) 9 | add_subdirectory(genericSensorClient) 10 | -------------------------------------------------------------------------------- /devices/wholeBodyDynamics/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include(WBEAddUnitTest) 2 | 3 | add_subdirectory(fakeFTs) 4 | 5 | wbe_add_unit_test(NAME WholeBodyDynamics 6 | SOURCES WholeBodyDynamicsUnitTests.cpp 7 | LINKS YARP::YARP_robotinterface) 8 | 9 | target_compile_definitions(WholeBodyDynamicsUnitTests 10 | PRIVATE CMAKE_CURRENT_SOURCE_DIR="${CMAKE_CURRENT_SOURCE_DIR}" 11 | PROJECT_BINARY_DIR="${PROJECT_BINARY_DIR}") 12 | -------------------------------------------------------------------------------- /devices/baseEstimatorV1/include/WalkingLogger.tpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file WalkingLogger.tpp 3 | * @authors Giulio Romualdi 4 | * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia 5 | * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT 6 | * @date 2018 7 | */ 8 | 9 | #include 10 | 11 | template 12 | void WalkingLogger::sendData(const Args&... args) 13 | { 14 | YarpHelper::sendVariadicVector(m_dataPort, args...); 15 | } 16 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | 34 | # Qtcreator project 35 | *.user 36 | 37 | # clangd 38 | **/compile_commands.json 39 | **/.cache/ 40 | # pixi environments 41 | .pixi* 42 | .build* 43 | -------------------------------------------------------------------------------- /devices/baseEstimatorV1/ros/fbeViz.launch: -------------------------------------------------------------------------------- 1 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /pixi_source_deps_options.meta: -------------------------------------------------------------------------------- 1 | { 2 | "names": 3 | { 4 | "YARP" : 5 | { 6 | "cmake-args": [ 7 | "-DCMAKE_BUILD_TYPE=RelWithDebInfo", 8 | "-GNinja", 9 | "-DYARP_COMPILE_ALL_FAKE_DEVICES:BOOL=ON", 10 | "-DYARP_COMPILE_GUIS=OFF" 11 | ] 12 | }, 13 | "ICUB" : 14 | { 15 | "cmake-args": [ 16 | "-DCMAKE_BUILD_TYPE=RelWithDebInfo", 17 | "-GNinja", 18 | "-DBUILD_SHARED_LIBS=ON", 19 | "-DICUBMAIN_COMPILE_CORE=OFF", 20 | "-DICUBMAIN_COMPILE_TOOLS=OFF", 21 | "-DICUBMAIN_COMPILE_MODULES=OFF" 22 | ] 23 | } 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /devices/wholeBodyDynamics/test/fakeFTs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | yarp_prepare_plugin(fakeFTs 2 | CATEGORY device 3 | TYPE fakeFTs 4 | INCLUDE fakeFTs.h 5 | DEFAULT ON 6 | INTERNAL 7 | ADVANCED 8 | ) 9 | 10 | yarp_add_plugin(fakeFTs) 11 | target_sources(fakeFTs 12 | PRIVATE 13 | fakeFTs.cpp 14 | fakeFTs.h 15 | ) 16 | target_link_libraries(fakeFTs 17 | PRIVATE 18 | YARP::YARP_os 19 | YARP::YARP_sig 20 | YARP::YARP_dev 21 | YARP::YARP_math 22 | ) 23 | 24 | yarp_install(TARGETS fakeFTs 25 | EXPORT Runtime 26 | COMPONENT runtime 27 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 28 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 29 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 30 | -------------------------------------------------------------------------------- /cmake/WBEAddUnitTest.cmake: -------------------------------------------------------------------------------- 1 | function(wbe_add_unit_test) 2 | set(options ) 3 | set(oneValueArgs NAME) 4 | set(multiValueArgs SOURCES LINKS) 5 | 6 | set(prefix "wbe_add_unit_test") 7 | 8 | cmake_parse_arguments(${prefix} 9 | "${options}" 10 | "${oneValueArgs}" 11 | "${multiValueArgs}" 12 | ${ARGN}) 13 | 14 | set(name ${${prefix}_NAME}) 15 | set(unit_test_files ${${prefix}_SOURCES}) 16 | 17 | set(targetname ${name}UnitTests) 18 | add_executable(${targetname} 19 | "${unit_test_files}") 20 | 21 | target_link_libraries(${targetname} PRIVATE Catch2::Catch2WithMain ${${prefix}_LINKS}) 22 | target_compile_definitions(${targetname} PRIVATE CATCH_CONFIG_FAST_COMPILE CATCH_CONFIG_DISABLE_MATCHERS) 23 | 24 | add_test(NAME ${targetname} COMMAND ${targetname}) 25 | endfunction() 26 | -------------------------------------------------------------------------------- /.github/workflows/pixi-auto-update-ci.yml: -------------------------------------------------------------------------------- 1 | name: Pixi auto update 2 | 3 | on: 4 | # on demand 5 | workflow_dispatch: 6 | 7 | jobs: 8 | auto-update: 9 | runs-on: ubuntu-latest 10 | steps: 11 | - uses: actions/checkout@v4 12 | 13 | - uses: prefix-dev/setup-pixi@v0.5.1 14 | with: 15 | pixi-version: "latest" 16 | cache: false 17 | - name: Update pixi lock file 18 | run: | 19 | rm pixi.lock 20 | pixi install 21 | - uses: peter-evans/create-pull-request@v5 22 | with: 23 | token: ${{ secrets.GITHUB_TOKEN }} 24 | branch: update/pixi-lock 25 | title: Update pixi lock file 26 | commit-message: "Update `pixi.lock`" 27 | body: Update pixi dependencies to the latest version. 28 | author: "GitHub " 29 | -------------------------------------------------------------------------------- /devices/baseEstimatorV1/app/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. 2 | # This software may be modified and distributed under the terms of the 3 | # GNU Lesser General Public License v2.1 or any later version. 4 | 5 | # Thanks to Stefano Dafarra for this CMakeLists.txt 6 | 7 | # List the subdirectory (http://stackoverflow.com/questions/7787823/cmake-how-to-get-the-name-of-all-subdirectories-of-a-directory) 8 | macro(SUBDIRLIST result curdir) 9 | file(GLOB children RELATIVE ${curdir} ${curdir}/*) 10 | set(dirlist "") 11 | foreach(child ${children}) 12 | if(IS_DIRECTORY ${curdir}/${child}) 13 | list(APPEND dirlist ${child}) 14 | endif() 15 | endforeach() 16 | set(${result} ${dirlist}) 17 | endmacro() 18 | 19 | # Get list of models 20 | subdirlist(subdirs ${CMAKE_CURRENT_SOURCE_DIR}/robots/) 21 | # Install each model 22 | foreach (dir ${subdirs}) 23 | yarp_install(DIRECTORY robots/${dir} DESTINATION ${YARP_ROBOTS_INSTALL_DIR}) 24 | endforeach () 25 | 26 | -------------------------------------------------------------------------------- /libraries/wholeBodyDynamicsHelpers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2016 Istituto Italiano di Tecnologia iCub Facility 2 | # Authors: Silvio Traversaro 3 | # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT 4 | 5 | project(wholeBodyDynamicsHelpers) 6 | 7 | set(WHOLEBODYDYNAMICS_HELPERS_HDRS include/wholeBodyDynamics/) 8 | 9 | set(WHOLEBODYDYNAMICS_HELPERS_SRCS src/) 10 | 11 | add_library(${PROJECT_NAME} ${WHOLEBODYDYNAMICS_HELPERS_HDRS} ${WHOLEBODYDYNAMICS_HELPERS_SRCS}) 12 | 13 | target_include_directories(${PROJECT_NAME} PUBLIC 14 | "$" 15 | "$") 16 | target_link_libraries(${PROJECT_NAME} ${YARP_LIBRARIES}) 17 | 18 | set_property(TARGET ${PROJECT_NAME} PROPERTY PUBLIC_HEADER ${WHOLEBODYDYNAMICS_HELPERS_SRCS}) 19 | 20 | install(TARGETS ${PROJECT_NAME} 21 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 22 | PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wholeBodyDynamics) 23 | 24 | -------------------------------------------------------------------------------- /devices/virtualAnalogClient/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: (C) 2016 Istituto Italiano di Tecnologia 2 | # Authors: Silvio Traversaro 3 | # CopyPolicy: Released under the terms of the GNU LGPL v2+ 4 | 5 | yarp_prepare_plugin(virtualAnalogClient CATEGORY device 6 | TYPE yarp::dev::VirtualAnalogClient 7 | INCLUDE "VirtualAnalogClient.h" 8 | INTERNAL 9 | ADVANCED) 10 | 11 | if(ENABLE_virtualAnalogClient) 12 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}) 13 | 14 | yarp_add_plugin(virtualAnalogClient VirtualAnalogClient.h VirtualAnalogClient.cpp) 15 | 16 | target_link_libraries(virtualAnalogClient ${YARP_LIBRARIES}) 17 | 18 | yarp_install(TARGETS virtualAnalogClient 19 | EXPORT ${PROJECT_NAME} 20 | COMPONENT runtime 21 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 22 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR}) 23 | 24 | yarp_install(FILES virtualAnalogClient.ini DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 25 | endif() 26 | -------------------------------------------------------------------------------- /devices/genericSensorClient/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: (C) 2016 Istituto Italiano di Tecnologia 2 | # Authors: Silvio Traversaro 3 | # CopyPolicy: Released under the terms of the GNU LGPL v2+ 4 | 5 | yarp_prepare_plugin(genericSensorClient CATEGORY device 6 | TYPE yarp::dev::GenericSensorClient 7 | INCLUDE "GenericSensorClient.h" 8 | INTERNAL 9 | ADVANCED) 10 | 11 | if(ENABLE_genericSensorClient) 12 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}) 13 | 14 | 15 | yarp_add_plugin(genericSensorClient GenericSensorClient.h GenericSensorClient.cpp) 16 | 17 | target_link_libraries(genericSensorClient ${YARP_LIBRARIES}) 18 | 19 | yarp_install(TARGETS genericSensorClient 20 | EXPORT ${PROJECT_NAME} 21 | COMPONENT runtime 22 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 23 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR}) 24 | 25 | yarp_install(FILES genericSensorClient.ini DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 26 | endif() 27 | -------------------------------------------------------------------------------- /devices/virtualAnalogRemapper/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: (C) 2016 Istituto Italiano di Tecnologia 2 | # Authors: Silvio Traversaro 3 | # CopyPolicy: Released under the terms of the GNU LGPL v2+ 4 | 5 | yarp_prepare_plugin(virtualAnalogRemapper CATEGORY device 6 | TYPE yarp::dev::VirtualAnalogRemapper 7 | INCLUDE "VirtualAnalogRemapper.h" 8 | INTERNAL 9 | ADVANCED) 10 | 11 | if(ENABLE_virtualAnalogRemapper) 12 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}) 13 | 14 | 15 | yarp_add_plugin(virtualAnalogRemapper VirtualAnalogRemapper.h VirtualAnalogRemapper.cpp) 16 | 17 | target_link_libraries(virtualAnalogRemapper ${YARP_LIBRARIES}) 18 | 19 | yarp_install(TARGETS virtualAnalogRemapper 20 | EXPORT ${PROJECT_NAME} 21 | COMPONENT runtime 22 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 23 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR}) 24 | 25 | yarp_install(FILES virtualAnalogRemapper.ini DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 26 | endif() 27 | 28 | -------------------------------------------------------------------------------- /devices/wholeBodyDynamics/test/ergocub_test_all.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 24 10 | (torso_pitch,torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll) 11 | 12 | 13 | 14 | 15 | 16 | (l_leg_ft, l_foot_front_ft, l_foot_rear_ft, r_leg_ft, r_foot_front_ft, r_foot_rear_ft, r_arm_ft, l_arm_ft) 17 | 18 | 19 | 20 | 21 | "waist_imu_0" 22 | "waist_imu_0" 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /libraries/ctrlLibRT/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2016 Istituto Italiano di Tecnologia iCub Facility 2 | # Authors: Silvio Traversaro 3 | # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT 4 | 5 | cmake_minimum_required(VERSION 3.5) 6 | 7 | project(ctrlLibRT) 8 | 9 | set(${PROJECT_NAME}_HDRS include/${PROJECT_NAME}/filters.h) 10 | 11 | set(${PROJECT_NAME}_SRCS src/filters.cpp) 12 | 13 | add_library(${PROJECT_NAME} STATIC ${${PROJECT_NAME}_HDRS} ${${PROJECT_NAME}_SRCS}) 14 | set_property(TARGET ${PROJECT_NAME} PROPERTY POSITION_INDEPENDENT_CODE ON) 15 | 16 | target_include_directories(${PROJECT_NAME} PUBLIC 17 | "$" 18 | "$") 19 | 20 | include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR}) 21 | target_include_directories(${PROJECT_NAME} PUBLIC ${EIGEN3_INCLUDE_DIR}) 22 | target_link_libraries(${PROJECT_NAME} ${YARP_LIBRARIES}) 23 | 24 | set_property(TARGET ${PROJECT_NAME} PROPERTY PUBLIC_HEADER ${${PROJECT_NAME}_HELPERS_SRCS}) 25 | 26 | if(MSVC) 27 | add_definitions(-D_USE_MATH_DEFINES) 28 | endif() 29 | 30 | install(TARGETS ${PROJECT_NAME} 31 | RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT bin 32 | LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT shlib 33 | ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT lib 34 | PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/${PROJECT_NAME}) 35 | 36 | -------------------------------------------------------------------------------- /idl/floatingBaseEstimatorRPC/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2016 Istituto Italiano di Tecnologia iCub Facility 2 | # Authors: Silvio Traversaro 3 | # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT 4 | 5 | yarp_idl_to_dir(INPUT_FILES codyco/floatingBaseEstimatorRPC.thrift 6 | OUTPUT_DIR ${CMAKE_CURRENT_BINARY_DIR}/autogenerated 7 | SOURCES_VAR WBDIDLSERVER_SRC 8 | HEADERS_VAR WBDIDLSERVER_HEADERS 9 | INCLUDE_DIRS_VAR WBDIDLSERVER_INCLUDES) 10 | 11 | add_library(floatingBaseEstimatorRPC ${WBDIDLSERVER_SRC} ${WBDIDLSERVER_HEADERS}) 12 | target_link_libraries(floatingBaseEstimatorRPC ${YARP_LIBRARIES}) 13 | 14 | target_include_directories(floatingBaseEstimatorRPC PUBLIC 15 | "$" 16 | "$") 17 | 18 | set_property(TARGET floatingBaseEstimatorRPC PROPERTY PUBLIC_HEADER ${WBDIDLSERVER_HEADERS}) 19 | 20 | install(TARGETS floatingBaseEstimatorRPC 21 | EXPORT ${VARS_PREFIX} 22 | RUNTIME DESTINATION "${CMAKE_INSTALL_FULL_BINDIR}" COMPONENT bin 23 | LIBRARY DESTINATION "${CMAKE_INSTALL_FULL_LIBDIR}" COMPONENT shlib 24 | ARCHIVE DESTINATION "${CMAKE_INSTALL_FULL_LIBDIR}" COMPONENT lib 25 | PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_FULL_INCLUDEDIR}/codyco) 26 | 27 | # Add to targets exported by CMake 28 | set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS floatingBaseEstimatorRPC) 29 | 30 | -------------------------------------------------------------------------------- /idl/VectorsCollection/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2016 Istituto Italiano di Tecnologia iCub Facility 2 | # Authors: Silvio Traversaro 3 | # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT 4 | 5 | cmake_minimum_required(VERSION 3.5) 6 | 7 | project(VectorsCollection) 8 | 9 | yarp_idl_to_dir(INPUT_FILES VectorsCollection.thrift 10 | OUTPUT_DIR ${CMAKE_CURRENT_BINARY_DIR}/autogenerated 11 | SOURCES_VAR WBDSETTINGS_SRC 12 | HEADERS_VAR WBDSETTINGS_HEADERS 13 | INCLUDE_DIRS_VAR WBDSETTINGS_INCLUDES) 14 | 15 | add_library(VectorsCollection STATIC ${WBDSETTINGS_SRC} ${WBDSETTINGS_HEADERS}) 16 | target_include_directories(VectorsCollection PUBLIC ${WBDSETTINGS_INCLUDES}) 17 | set_property(TARGET VectorsCollection PROPERTY POSITION_INDEPENDENT_CODE ON) 18 | target_link_libraries(VectorsCollection ${YARP_LIBRARIES}) 19 | 20 | target_include_directories(VectorsCollection PUBLIC 21 | "$" 22 | "$") 23 | 24 | set_property(TARGET VectorsCollection PROPERTY PUBLIC_HEADER ${WBDSETTINGS_HEADERS}) 25 | 26 | install(TARGETS VectorsCollection 27 | RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT bin 28 | LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT shlib 29 | ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT lib 30 | PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wholeBodyDynamics) 31 | 32 | -------------------------------------------------------------------------------- /.github/workflows/pixi-ci.yml: -------------------------------------------------------------------------------- 1 | name: CI Workflow 2 | 3 | on: 4 | # on demand 5 | workflow_dispatch: 6 | inputs: 7 | delete_pixi_lock: 8 | description: 'If true, delete pixi.lock, to test against the latest version of dependencies.' 9 | required: true 10 | default: 'false' 11 | pull_request: 12 | schedule: 13 | # * is a special character in YAML so you have to quote this string 14 | # Execute a "nightly" build twice a week 2 AM UTC 15 | - cron: '0 2 * * 2,5' 16 | 17 | jobs: 18 | build-with-pixi: 19 | name: '[pixi:${{ matrix.os }}]' 20 | runs-on: ${{ matrix.os }} 21 | strategy: 22 | fail-fast: false 23 | matrix: 24 | build_type: [Release] 25 | os: [ubuntu-22.04, macos-13, windows-2019] 26 | 27 | steps: 28 | - uses: actions/checkout@v4 29 | 30 | # On periodic jobs and when delete_pixi_lock option is true, delete the pixi.lock to check that the project compiles with latest version of dependencies 31 | - name: Delete pixi.lock on scheduled jobs or if delete_pixi_lock is true 32 | if: github.event_name == 'schedule' || (github.event_name == 'workflow_dispatch' && github.event.inputs.delete_pixi_lock == 'true') 33 | shell: bash 34 | run: | 35 | rm pixi.lock 36 | 37 | - name: Print used environment 38 | shell: bash 39 | run: | 40 | env 41 | 42 | - uses: prefix-dev/setup-pixi@v0.5.1 43 | 44 | - name: Run the tests 45 | shell: bash -l {0} 46 | run: | 47 | pixi run download_deps 48 | pixi run install_deps 49 | pixi run test 50 | -------------------------------------------------------------------------------- /devices/baseEstimatorV1/thrifts/floatingBaseEstimationRPC.thrift: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT) 3 | * All rights reserved. 4 | * 5 | * This software may be modified and distributed under the terms of the 6 | * GNU Lesser General Public License v2.1 or any later version. 7 | */ 8 | 9 | struct Pose6D 10 | { 11 | 1: double x; 2: double y; 3: double z; 12 | 4: double roll; 5: double pitch; 6: double yaw; 13 | } 14 | 15 | service floatingBaseEstimationRPC 16 | { 17 | string getEstimationJointsList(); 18 | bool setMahonyKp(1: double kp); 19 | bool setMahonyKi(1: double ki); 20 | bool setMahonyTimeStep(1: double timestep); 21 | bool setContactSchmittThreshold(1: double l_fz_break, 2: double l_fz_make, 22 | 3: double r_fz_break, 4: double r_fz_make); 23 | 24 | bool setPrimaryFoot(1: string primary_foot); 25 | 26 | bool resetIMU(); 27 | bool resetLeggedOdometry(); 28 | bool resetLeggedOdometryWithRefFrame(1: string ref_frame, 2: double x, 3: double y, 4: double z, 29 | 5: double roll, 6: double pitch, 7: double yaw); 30 | bool resetEstimator(); 31 | bool resetEstimatorWithRefFrame(1: string ref_frame, 2: double x, 3: double y, 4: double z, 32 | 5: double roll, 6: double pitch, 7: double yaw); 33 | 34 | string getRefFrameForWorld(); 35 | Pose6D getRefPose6DForWorld(); 36 | bool useJointVelocityLPF(1: bool flag); 37 | bool setJointVelocityLPFCutoffFrequency(1: double freq); 38 | bool startFloatingBaseFilter(); 39 | } 40 | 41 | -------------------------------------------------------------------------------- /devices/baseEstimatorV1/include/WalkingLogger.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file WalkingLogger.hpp 3 | * @authors Giulio Romualdi 4 | * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia 5 | * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT 6 | * @date 2018 7 | */ 8 | 9 | #ifndef WALKING_LOGGER_HPP 10 | #define WALKING_LOGGER_HPP 11 | 12 | // YARP 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | class WalkingLogger 19 | { 20 | yarp::os::BufferedPort m_dataPort; /**< Data logger port. */ 21 | yarp::os::RpcClient m_rpcPort; /**< RPC data logger port. */ 22 | 23 | public: 24 | 25 | /** 26 | * Configure 27 | * @param config yarp searchable configuration variable; 28 | * @param name is the name of the module. 29 | * @return true/false in case of success/failure. 30 | */ 31 | bool configure(const yarp::os::Searchable& config, const std::string& name); 32 | 33 | /** 34 | * Start record. 35 | * @param strings head of the logger file 36 | * @return true/false in case of success/failure. 37 | */ 38 | bool startRecord(const std::initializer_list& strings); 39 | 40 | /** 41 | * Quit the logger. 42 | */ 43 | void quit(); 44 | 45 | /** 46 | * Send data to the logger. 47 | * @param args all the vector containing the data that will be sent. 48 | */ 49 | template 50 | void sendData(const Args&... args); 51 | }; 52 | 53 | #include "WalkingLogger.tpp" 54 | 55 | #endif 56 | -------------------------------------------------------------------------------- /idl/wholeBodyDynamicsSettings/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2016 Istituto Italiano di Tecnologia iCub Facility 2 | # Authors: Silvio Traversaro 3 | # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT 4 | 5 | cmake_minimum_required(VERSION 3.5) 6 | 7 | project(wholeBodyDynamicsSettings) 8 | 9 | yarp_idl_to_dir(INPUT_FILES wholeBodyDynamicsSettings.thrift 10 | OUTPUT_DIR ${CMAKE_CURRENT_BINARY_DIR}/autogenerated 11 | SOURCES_VAR WBDSETTINGS_SRC 12 | HEADERS_VAR WBDSETTINGS_HEADERS 13 | INCLUDE_DIRS_VAR WBDSETTINGS_INCLUDES) 14 | 15 | add_library(wholeBodyDynamicsSettings STATIC ${WBDSETTINGS_SRC} ${WBDSETTINGS_HEADERS}) 16 | target_include_directories(wholeBodyDynamicsSettings PUBLIC ${WBDSETTINGS_INCLUDES}) 17 | set_property(TARGET wholeBodyDynamicsSettings PROPERTY POSITION_INDEPENDENT_CODE ON) 18 | target_link_libraries(wholeBodyDynamicsSettings ${YARP_LIBRARIES}) 19 | 20 | target_include_directories(wholeBodyDynamicsSettings PUBLIC 21 | "$" 22 | "$") 23 | 24 | set_property(TARGET wholeBodyDynamicsSettings PROPERTY PUBLIC_HEADER ${WBDSETTINGS_HEADERS}) 25 | 26 | install(TARGETS wholeBodyDynamicsSettings 27 | RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT bin 28 | LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT shlib 29 | ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT lib 30 | PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wholeBodyDynamics) 31 | 32 | -------------------------------------------------------------------------------- /idl/wholeBodyDynamics_IDLServer/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2016 Istituto Italiano di Tecnologia iCub Facility 2 | # Authors: Silvio Traversaro 3 | # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT 4 | 5 | cmake_minimum_required(VERSION 3.5) 6 | 7 | project(wholeBodyDynamics_IDLServer) 8 | 9 | yarp_idl_to_dir(INPUT_FILES wholeBodyDynamics_IDLServer.thrift 10 | OUTPUT_DIR ${CMAKE_CURRENT_BINARY_DIR}/autogenerated 11 | SOURCES_VAR WBDIDLSERVER_SRC 12 | HEADERS_VAR WBDIDLSERVER_HEADERS 13 | INCLUDE_DIRS_VAR WBDIDLSERVER_INCLUDES) 14 | 15 | add_library(wholeBodyDynamics_IDLServer STATIC ${WBDIDLSERVER_SRC} ${WBDIDLSERVER_HEADERS}) 16 | target_include_directories(wholeBodyDynamics_IDLServer PUBLIC ${WBDIDLSERVER_INCLUDES}) 17 | set_property(TARGET wholeBodyDynamics_IDLServer PROPERTY POSITION_INDEPENDENT_CODE ON) 18 | target_link_libraries(wholeBodyDynamics_IDLServer ${YARP_LIBRARIES}) 19 | 20 | target_include_directories(wholeBodyDynamics_IDLServer PUBLIC 21 | "$" 22 | "$") 23 | 24 | set_property(TARGET wholeBodyDynamics_IDLServer PROPERTY PUBLIC_HEADER ${WBDIDLSERVER_HEADERS}) 25 | 26 | install(TARGETS wholeBodyDynamics_IDLServer 27 | RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT bin 28 | LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT shlib 29 | ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT lib 30 | PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wholeBodyDynamics) 31 | 32 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. 2 | # This software may be modified and distributed under the terms of the 3 | # GNU Lesser General Public License v2.1 or any later version. 4 | 5 | cmake_minimum_required(VERSION 3.5...3.22) 6 | project(whole-body-estimators LANGUAGES CXX 7 | VERSION 0.11.3) 8 | 9 | set(CMAKE_CXX_STANDARD 14) 10 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 11 | 12 | ## avoid having M_PI error by importing math symbols from standard cmath 13 | add_definitions(-D_USE_MATH_DEFINES) 14 | # and export all symbols in Windows 15 | set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) 16 | 17 | 18 | option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON) 19 | 20 | list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) 21 | 22 | option(ENABLE_RPATH "Enable RPATH for this library" ON) 23 | mark_as_advanced(ENABLE_RPATH) 24 | include(AddInstallRPATHSupport) 25 | add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_BINDIR}" 26 | LIB_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}" 27 | INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}" 28 | DEPENDS ENABLE_RPATH 29 | USE_LINK_PATH) 30 | 31 | set(YARP_REQUIRED_VERSION 3.0.1) 32 | 33 | find_package(YARP ${YARP_REQUIRED_VERSION} REQUIRED COMPONENTS robotinterface idl_tools eigen os sig) 34 | find_package(Eigen3 REQUIRED) 35 | find_package(iDynTree 12.2.0 REQUIRED) 36 | find_package(ICUB REQUIRED) 37 | 38 | yarp_configure_plugins_installation(${PROJECT_NAME}) 39 | 40 | option(BUILD_TESTING "Create tests using CMake" OFF) 41 | if(BUILD_TESTING) 42 | find_package(Catch2 3.0.1 REQUIRED) 43 | enable_testing() 44 | endif() 45 | 46 | add_subdirectory(libraries) 47 | add_subdirectory(idl) 48 | add_subdirectory(devices) 49 | add_subdirectory(scripts) 50 | 51 | include(AddUninstallTarget) 52 | 53 | -------------------------------------------------------------------------------- /devices/wholeBodyDynamics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: (C) 2016 Istituto Italiano di Tecnologia 2 | # Authors: Silvio Traversaro 3 | # CopyPolicy: Released under the terms of the GNU LGPL v2+ 4 | 5 | 6 | yarp_prepare_plugin(wholebodydynamics CATEGORY device 7 | TYPE yarp::dev::WholeBodyDynamicsDevice 8 | INCLUDE "WholeBodyDynamicsDevice.h" 9 | INTERNAL 10 | ADVANCED) 11 | 12 | if(ENABLE_wholebodydynamics) 13 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}) 14 | 15 | include_directories(SYSTEM 16 | ${EIGEN3_INCLUDE_DIR} 17 | ${skinDynLib_INCLUDE_DIRS}) 18 | 19 | yarp_add_plugin(wholeBodyDynamicsDevice WholeBodyDynamicsDevice.h WholeBodyDynamicsDevice.cpp 20 | SixAxisForceTorqueMeasureHelpers.h SixAxisForceTorqueMeasureHelpers.cpp 21 | GravityCompensationHelpers.h GravityCompensationHelpers.cpp) 22 | 23 | target_link_libraries(wholeBodyDynamicsDevice wholeBodyDynamicsSettings 24 | wholeBodyDynamics_IDLServer 25 | VectorsCollection 26 | ctrlLibRT 27 | ${YARP_LIBRARIES} 28 | skinDynLib 29 | ${iDynTree_LIBRARIES}) 30 | 31 | if(MSVC) 32 | add_definitions(-D_USE_MATH_DEFINES) 33 | endif() 34 | 35 | 36 | yarp_install(TARGETS wholeBodyDynamicsDevice 37 | EXPORT Runtime 38 | COMPONENT runtime 39 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 40 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR}) 41 | 42 | yarp_install(FILES wholebodydynamics.ini DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 43 | 44 | # Install configuration files 45 | add_subdirectory(app) 46 | 47 | if(BUILD_TESTING) 48 | add_subdirectory(test) 49 | endif() 50 | endif() 51 | -------------------------------------------------------------------------------- /idl/wholeBodyDynamicsSettings/wholeBodyDynamicsSettings.thrift: -------------------------------------------------------------------------------- 1 | struct Gravity { 2 | 1: double x; /* x gravity component [m/s^2] */ 3 | 2: double y; /* y gravity component [m/s^2] */ 4 | 3: double z; /* z gravity component [m/s^2] */ 5 | } 6 | 7 | struct ContactPoint { 8 | 1: double x; /* x component [m] */ 9 | 2: double y; /* y component [m] */ 10 | 3: double z; /* z component [m] */ 11 | } 12 | 13 | enum KinematicSourceType { 14 | IMU, 15 | FIXED_FRAME 16 | } 17 | 18 | struct wholeBodyDynamicsSettings { 19 | 1: KinematicSourceType kinematicSource; /** Specify the source of the kinematic information for one link, see KinematicSourceType information for more info. */ 20 | 2: string fixedFrameName; /** If kinematicSource is FIXED_LINK, specify the frame of the robot that we know to be fixed (i.e. not moving with respect to an inertial frame) */ 21 | 3: Gravity fixedFrameGravity; /** If kinematicSource is FIXED_LINK, specify the gravity vector (in m/s^2) in the fixedFrame */ 22 | 4: string imuFrameName; /** If kinematicSource is IMU, specify the frame name of the imu */ 23 | 5: double imuFilterCutoffInHz; /** Cutoff frequency (in Hz) of the first order filter of the IMU */ 24 | 6: double forceTorqueFilterCutoffInHz; /** Cutoff frequency(in Hz) of the first order filter of the F/T sensors */ 25 | 7: double jointVelFilterCutoffInHz; /** Cutoff frequency(in Hz) of the first order filter of the joint velocities */ 26 | 8: double jointAccFilterCutoffInHz; /** Cutoff frequency(in Hz) of the first order filter of the joint accelerations */ 27 | 9: bool useJointVelocity; /** Use the joint velocity measurement if this is true, assume they are zero otherwise. */ 28 | 10: bool useJointAcceleration; /** Use the joint acceleration measurment if this is true, assume they are zero otherwise. */ 29 | 11: bool startWithZeroFTSensorOffsets /** Use zero FT sensor offsets at startup. If this flag is set to false, the device estimates the offsets of FT sensors at startup. Note that this option allows to enable/disable skipping the manual calling of resetOffsets to reset the offsets for FT sensors, most specially during simulations*/ 30 | /** If this flag is set to true, the read from the sensors is skipped at startup */ 31 | 12: bool disableSensorReadCheckAtStartup 32 | 13: bool estimateJointVelocityAcceleration; /** Estimate joint velocity and acceleration if this is true, use measurements otherwise. */ 33 | } ( 34 | yarp.editor = "true" 35 | ) 36 | -------------------------------------------------------------------------------- /pixi.toml: -------------------------------------------------------------------------------- 1 | [project] 2 | name = "whole-body-estimators" 3 | # As this version is currently ignored, we do not 4 | # waste effort in mantain it in synch with the value 5 | # specified in CMakeLists.txt 6 | version = "0.0.0" 7 | description = "YARP devices that implement estimators for humanoid robots." 8 | authors = ["Silvio Traversaro "] 9 | channels = ["conda-forge","robotology"] 10 | platforms = ["linux-64", "win-64", "osx-64"] 11 | 12 | # Workaround until https://gitlab.kitware.com/cmake/cmake/-/merge_requests/9200 13 | # is released 14 | 15 | [activation] 16 | scripts = ["pixi_activation.sh"] 17 | 18 | [target.win-64.activation] 19 | scripts = ["pixi_activation.bat"] 20 | 21 | 22 | [tasks] 23 | 24 | download_deps = {cmd = "echo Download all deps && mkdir -p ./.pixi_colcon_ws/src && vcs import --input ./pixi_source_deps.yaml ./.pixi_colcon_ws/src"} 25 | install_deps = {cmd = "echo Install all deps && cd .pixi_colcon_ws && colcon build --metas ../pixi_source_deps_options.meta --event-handlers console_direct+ --merge-install --install-base $CMAKE_INSTALL_PREFIX"} 26 | 27 | configure = { cmd = [ 28 | "cmake", 29 | "-DCMAKE_BUILD_TYPE=Release", 30 | "-DCMAKE_INSTALL_PREFIX=$CMAKE_INSTALL_PREFIX", 31 | "-DBUILD_TESTING:BOOL=ON", 32 | # Use the cross-platform Ninja generator 33 | "-G", 34 | "Ninja", 35 | # The source is in the root directory 36 | "-S", 37 | ".", 38 | # We wanna build in the .build directory 39 | "-B", 40 | ".build", 41 | ]} 42 | 43 | build = { cmd = "cmake --build .build --config Release", depends_on = ["configure"] } 44 | test = { cmd = "ctest --test-dir .build --build-config Release --output-on-failure", depends_on = ["build"] } 45 | install = { cmd = ["cmake", "--install", ".build", "--config", "Release"], depends_on = ["build"] } 46 | uninstall = { cmd = ["cmake", "--build", ".build", "--target", "uninstall"]} 47 | 48 | 49 | [dependencies] 50 | c-compiler = "*" 51 | cxx-compiler = "*" 52 | ninja = "*" 53 | cmake = "*" 54 | make = "*" 55 | pkg-config = "*" 56 | eigen = "*" 57 | # Requires https://github.com/robotology/idyntree/pull/1178 58 | idyntree = ">=12.2.0" 59 | # YARP from source dependencies (used only when compiling idyntree from source) 60 | ace = "*" 61 | ycm-cmake-modules = "*" 62 | catch2 = "*" 63 | tinyxml = "*" 64 | # iDynTree deps from source (used only when compiling idyntree from source) 65 | libxml2 = "*" 66 | assimp = "*" 67 | ipopt = "*" 68 | # Used to download and build dependencies from source 69 | colcon-common-extensions = "*" 70 | vcstool = "*" 71 | -------------------------------------------------------------------------------- /devices/baseEstimatorV1/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. 2 | # This software may be modified and distributed under the terms of the 3 | # GNU Lesser General Public License v2.1 or any later version. 4 | 5 | 6 | set(FBE_HEADERS ${CMAKE_CURRENT_SOURCE_DIR}/include/baseEstimatorV1.h 7 | ${CMAKE_CURRENT_SOURCE_DIR}/include/WalkingLogger.tpp 8 | ${CMAKE_CURRENT_SOURCE_DIR}/include/WalkingLogger.hpp 9 | ${CMAKE_CURRENT_SOURCE_DIR}/include/Utils.tpp 10 | ${CMAKE_CURRENT_SOURCE_DIR}/include/Utils.hpp) 11 | 12 | set(FBE_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/baseEstimatorV1.cpp 13 | ${CMAKE_CURRENT_SOURCE_DIR}/src/fbeRobotInterface.cpp 14 | ${CMAKE_CURRENT_SOURCE_DIR}/src/configureEstimator.cpp 15 | ${CMAKE_CURRENT_SOURCE_DIR}/src/WalkingLogger.cpp 16 | ${CMAKE_CURRENT_SOURCE_DIR}/src/Utils.cpp) 17 | 18 | yarp_prepare_plugin(baseEstimatorV1 CATEGORY device 19 | TYPE yarp::dev::baseEstimatorV1 20 | INCLUDE ${FBE_HEADERS} 21 | INTERNAL) 22 | 23 | yarp_add_idl(THRIFT "${CMAKE_CURRENT_SOURCE_DIR}/thrifts/floatingBaseEstimationRPC.thrift") 24 | add_library(floatingBaseEstimationRPC-service STATIC ${THRIFT}) 25 | target_include_directories(floatingBaseEstimationRPC-service PUBLIC ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_BINDIR}/include) 26 | target_link_libraries(floatingBaseEstimationRPC-service YARP::YARP_init YARP::YARP_OS) 27 | set_property(TARGET floatingBaseEstimationRPC-service PROPERTY POSITION_INDEPENDENT_CODE ON) 28 | 29 | yarp_add_plugin(baseEstimatorV1 ${FBE_SOURCES} ${FBE_HEADERS}) 30 | 31 | target_include_directories(baseEstimatorV1 PRIVATE 32 | ${CMAKE_CURRENT_SOURCE_DIR}/include) 33 | 34 | target_link_libraries(baseEstimatorV1 PUBLIC ${YARP_LIBRARIES} 35 | YARP::YARP_eigen 36 | YARP::YARP_os 37 | YARP::YARP_sig 38 | YARP::YARP_dev 39 | ctrlLib 40 | ${iDynTree_LIBRARIES} 41 | floatingBaseEstimationRPC-service 42 | PRIVATE Eigen3::Eigen) 43 | 44 | yarp_install(TARGETS baseEstimatorV1 45 | COMPONENT Runtime 46 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR}/ 47 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR}/) 48 | 49 | yarp_install(FILES baseEstimatorV1.ini 50 | COMPONENT runtime 51 | DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}/) 52 | 53 | add_subdirectory(app) 54 | 55 | -------------------------------------------------------------------------------- /devices/wholeBodyDynamics/test/WholeBodyDynamicsUnitTests.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #include 5 | 6 | // yarprobotinterface 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | TEST_CASE("WholeBodyDynamicsSmokeTest") 15 | { 16 | // Add PROJECT_BINARY_DIR/share to YARP_DATA_DIRS to find fakeFTs and wholebodydynamics and fakeFTs devices 17 | // from the build directory 18 | std::string oldyarpdatadirs = getenv("YARP_DATA_DIRS"); 19 | 20 | #ifdef _WIN32 21 | std::string pathsep = ";"; 22 | #else 23 | std::string pathsep = ":"; 24 | #endif 25 | std::string newyarpdatadirs = std::string("YARP_DATA_DIRS=")+std::string(PROJECT_BINARY_DIR)+"/share/yarp"+pathsep+oldyarpdatadirs; 26 | #ifdef _WIN32 27 | _putenv(newyarpdatadirs.c_str()); 28 | #else 29 | putenv(const_cast(newyarpdatadirs.c_str())); 30 | #endif 31 | 32 | // Set YARP_ROBOT_NAME to ensure that the correct ergocub file is found by wholebodydynamics 33 | std::string newyarprobotname = std::string("YARP_ROBOT_NAME=ergoCubSN001"); 34 | #ifdef _WIN32 35 | _putenv(newyarprobotname.c_str()); 36 | #else 37 | putenv(const_cast(newyarprobotname.c_str())); 38 | #endif 39 | 40 | // Avoid to need yarpserver 41 | yarp::os::NetworkBase::setLocalMode(true); 42 | 43 | // Load the yarprobotinterface ergocub_test_all.xml file, and check for errors 44 | yarp::os::Network yarpInit; 45 | yarp::robotinterface::XMLReader xmlRobotInterfaceReader; 46 | yarp::robotinterface::XMLReaderResult xmlRobotInterfaceResult; 47 | std::string location_ergocub_test_all = std::string(CMAKE_CURRENT_SOURCE_DIR) + std::string("/ergocub_test_all.xml"); 48 | 49 | xmlRobotInterfaceResult = xmlRobotInterfaceReader.getRobotFromFile(location_ergocub_test_all); 50 | 51 | REQUIRE(xmlRobotInterfaceResult.parsingIsSuccessful); 52 | 53 | // Enter the startup phase, that will open all the devices and call attach if necessary 54 | REQUIRE(xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseStartup)); 55 | REQUIRE(xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseRun)); 56 | 57 | // If the robotinterface started, we are good to go, we can close it 58 | REQUIRE(xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseInterrupt1)); 59 | REQUIRE(xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseInterrupt2)); 60 | REQUIRE(xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseInterrupt3)); 61 | REQUIRE(xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseShutdown)); 62 | } 63 | -------------------------------------------------------------------------------- /devices/wholeBodyDynamics/app/launch-wholebodydynamics-icub3-sim.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | /icubSim/torso 8 | /wholeBodyDynamics/torso 9 | 10 | 11 | 12 | /icubSim/left_arm 13 | /wholeBodyDynamics/left_arm 14 | 15 | 16 | 17 | /icubSim/right_arm 18 | /wholeBodyDynamics/right_arm 19 | 20 | 21 | 22 | /icubSim/left_leg 23 | /wholeBodyDynamics/left_leg 24 | 25 | 26 | 27 | /icubSim/right_leg 28 | /wholeBodyDynamics/right_leg 29 | 30 | 31 | 32 | /icubSim/head 33 | /wholeBodyDynamics/head 34 | 35 | 36 | 37 | 38 | /icubSim/head/inertials 39 | /wholeBodyDynamics/imu 40 | 41 | 42 | 43 | 44 | /icubSim/left_arm/FT 45 | /wholeBodyDynamics/l_arm_ft 46 | 47 | 48 | 49 | /icubSim/right_arm/FT 50 | /wholeBodyDynamics/r_arm_ft 51 | 52 | 53 | 54 | /icubSim/left_leg/FT 55 | /wholeBodyDynamics/l_leg_ft 56 | 57 | 58 | 59 | /icubSim/right_leg/FT 60 | /wholeBodyDynamics/r_leg_ft 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /devices/baseEstimatorV1/src/WalkingLogger.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file WalkingLogger.cpp 3 | * @authors Giulio Romualdi 4 | * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia 5 | * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT 6 | * @date 2018 7 | */ 8 | 9 | // YARP 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | bool WalkingLogger::configure(const yarp::os::Searchable& config, const std::string& name) 16 | { 17 | std::string portInput, portOutput; 18 | 19 | // check if the config file is empty 20 | if(config.isNull()) 21 | { 22 | yError() << "[configureLogger] Empty configuration for the force torque sensors."; 23 | return false; 24 | } 25 | 26 | // open the connect the data logger port 27 | if(!YarpHelper::getStringFromSearchable(config, "dataLoggerOutputPort_name", portOutput)) 28 | { 29 | yError() << "[configureLogger] Unable to get the string from searchable."; 30 | return false; 31 | } 32 | if(!YarpHelper::getStringFromSearchable(config, "dataLoggerInputPort_name", portInput)) 33 | { 34 | yError() << "[configureLogger] Unable to get the string from searchable."; 35 | return false; 36 | } 37 | m_dataPort.open("/" + name + portOutput); 38 | if(!yarp::os::Network::connect("/" + name + portOutput, portInput)) 39 | { 40 | yError() << "Unable to connect to port " << "/" + name + portOutput; 41 | return false; 42 | } 43 | 44 | // open the connect the rpc logger port 45 | if(!YarpHelper::getStringFromSearchable(config, "dataLoggerRpcOutputPort_name", portOutput)) 46 | { 47 | yError() << "[configureLogger] Unable to get the string from searchable."; 48 | return false; 49 | } 50 | if(!YarpHelper::getStringFromSearchable(config, "dataLoggerRpcInputPort_name", portInput)) 51 | { 52 | yError() << "[configureLogger] Unable to get the string from searchable."; 53 | return false; 54 | } 55 | m_rpcPort.open("/" + name + portOutput); 56 | if(!yarp::os::Network::connect("/" + name + portOutput, portInput)) 57 | { 58 | yError() << "Unable to connect to port " << "/" + name + portOutput; 59 | return false; 60 | } 61 | return true; 62 | } 63 | 64 | bool WalkingLogger::startRecord(const std::initializer_list& strings) 65 | { 66 | yarp::os::Bottle cmd, outcome; 67 | 68 | YarpHelper::populateBottleWithStrings(cmd, strings); 69 | 70 | m_rpcPort.write(cmd, outcome); 71 | if(outcome.get(0).asInt32() != 1) 72 | { 73 | yError() << "[startWalking] Unable to store data"; 74 | return false; 75 | } 76 | return true; 77 | } 78 | 79 | void WalkingLogger::quit() 80 | { 81 | // stop recording 82 | yarp::os::Bottle cmd, outcome; 83 | cmd.addString("quit"); 84 | m_rpcPort.write(cmd, outcome); 85 | if(outcome.get(0).asInt32() != 1) 86 | yInfo() << "[close] Unable to close the stream."; 87 | 88 | // close ports 89 | m_dataPort.close(); 90 | m_rpcPort.close(); 91 | } 92 | -------------------------------------------------------------------------------- /devices/wholeBodyDynamics/SixAxisForceTorqueMeasureHelpers.cpp: -------------------------------------------------------------------------------- 1 | #include "SixAxisForceTorqueMeasureHelpers.h" 2 | #include 3 | 4 | 5 | namespace wholeBodyDynamics 6 | { 7 | 8 | SixAxisForceTorqueMeasureProcessor::SixAxisForceTorqueMeasureProcessor() 9 | { 10 | // Initialize the affice function to be the identity 11 | toEigen(m_secondaryCalibrationMatrix).setIdentity(); 12 | m_offset.zero(); 13 | toEigen(m_temperatureCoefficients).setZero(); 14 | m_tempOffset=0.0; 15 | m_estimated_offset.zero(); 16 | } 17 | 18 | iDynTree::Matrix6x6& SixAxisForceTorqueMeasureProcessor::secondaryCalibrationMatrix() 19 | { 20 | return m_secondaryCalibrationMatrix; 21 | } 22 | 23 | iDynTree::Vector6& SixAxisForceTorqueMeasureProcessor::temperatureCoefficients() 24 | { 25 | return m_temperatureCoefficients; 26 | } 27 | 28 | 29 | const iDynTree::Matrix6x6& SixAxisForceTorqueMeasureProcessor::secondaryCalibrationMatrix() const 30 | { 31 | return m_secondaryCalibrationMatrix; 32 | } 33 | 34 | const iDynTree::Vector6& SixAxisForceTorqueMeasureProcessor::temperatureCoefficients() const 35 | { 36 | return m_temperatureCoefficients; 37 | } 38 | 39 | iDynTree::Wrench& SixAxisForceTorqueMeasureProcessor::offset() 40 | { 41 | return m_offset; 42 | } 43 | 44 | const iDynTree::Wrench& SixAxisForceTorqueMeasureProcessor::offset() const 45 | { 46 | return m_offset; 47 | } 48 | 49 | iDynTree::Wrench& SixAxisForceTorqueMeasureProcessor::estimatedOffset() 50 | { 51 | return m_estimated_offset; 52 | } 53 | 54 | const iDynTree::Wrench& SixAxisForceTorqueMeasureProcessor::estimatedOffset() const 55 | { 56 | return m_estimated_offset; 57 | } 58 | 59 | double& SixAxisForceTorqueMeasureProcessor::tempOffset() 60 | { 61 | return m_tempOffset; 62 | } 63 | 64 | const double& SixAxisForceTorqueMeasureProcessor::tempOffset() const 65 | { 66 | return m_tempOffset; 67 | } 68 | 69 | iDynTree::Wrench SixAxisForceTorqueMeasureProcessor::filt(const iDynTree::Wrench& input) const 70 | { 71 | return filt(input, 0.0); 72 | } 73 | 74 | iDynTree::Wrench SixAxisForceTorqueMeasureProcessor::filt(const iDynTree::Wrench & input, const double & temperature) const 75 | { 76 | Eigen::Matrix retEig = toEigen(m_secondaryCalibrationMatrix)*toEigen(input)-toEigen(m_offset)+toEigen(m_temperatureCoefficients)*(temperature-m_tempOffset); 77 | iDynTree::Wrench ret; 78 | fromEigen(ret,retEig); 79 | 80 | 81 | return ret; 82 | } 83 | 84 | iDynTree::Wrench SixAxisForceTorqueMeasureProcessor::applySecondaryCalibrationMatrix(const iDynTree::Wrench& input) const 85 | { 86 | return applySecondaryCalibrationMatrix( input, 0.0); 87 | } 88 | 89 | iDynTree::Wrench SixAxisForceTorqueMeasureProcessor::applySecondaryCalibrationMatrix(const iDynTree::Wrench& input, const double & temperature) const 90 | { 91 | Eigen::Matrix retEig = toEigen(m_secondaryCalibrationMatrix)*toEigen(input)+toEigen(m_temperatureCoefficients)*(temperature-m_tempOffset); 92 | 93 | iDynTree::Wrench ret; 94 | fromEigen(ret,retEig); 95 | 96 | return ret; 97 | } 98 | 99 | } 100 | -------------------------------------------------------------------------------- /devices/baseEstimatorV1/include/Utils.tpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Utils.tpp 3 | * @authors Giulio Romualdi 4 | * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia 5 | * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT 6 | * @date 2018 7 | */ 8 | 9 | // std 10 | #include 11 | 12 | // YARP 13 | #include 14 | 15 | template 16 | bool YarpHelper::yarpListToiDynTreeVectorFixSize(const yarp::os::Value& input, iDynTree::VectorFixSize& output) 17 | { 18 | if (input.isNull()) 19 | { 20 | yError() << "[yarpListToiDynTreeVectorFixSize] Empty input value."; 21 | return false; 22 | } 23 | if (!input.isList() || !input.asList()) 24 | { 25 | yError() << "[yarpListToiDynTreeVectorFixSize] Unable to read the input list."; 26 | return false; 27 | } 28 | yarp::os::Bottle *inputPtr = input.asList(); 29 | 30 | if (inputPtr->size() != n) 31 | { 32 | yError() << "[yarpListToiDynTreeVectorFixSize] The dimension set in the configuration file is not " 33 | << n; 34 | return false; 35 | } 36 | 37 | for (int i = 0; i < inputPtr->size(); i++) 38 | { 39 | if (!inputPtr->get(i).isFloat64() && !inputPtr->get(i).isInt32()) 40 | { 41 | yError() << "[yarpListToiDynTreeVectorFixSize] The input is expected to be a double"; 42 | return false; 43 | } 44 | output(i) = inputPtr->get(i).asFloat64(); 45 | } 46 | return true; 47 | } 48 | 49 | template 50 | void YarpHelper::mergeSigVector(yarp::sig::Vector& vector, const T& t) 51 | { 52 | for(int i= 0; i 59 | void YarpHelper::mergeSigVector(yarp::sig::Vector& vector, const T& t, const Args&... args) 60 | { 61 | for(int i= 0; i 70 | void YarpHelper::sendVariadicVector(yarp::os::BufferedPort& port, const Args&... args) 71 | { 72 | yarp::sig::Vector& vector = port.prepare(); 73 | vector.clear(); 74 | 75 | mergeSigVector(vector, args...); 76 | 77 | port.write(); 78 | } 79 | 80 | template 81 | bool StdHelper::appendVectorToDeque(const std::vector& input, std::deque& output, const size_t& initPoint) 82 | { 83 | if(initPoint > output.size()) 84 | { 85 | std::cerr << "[appendVectorToDeque] The init point has to be less or equal to the size of the output deque." 86 | << std::endl; 87 | return false; 88 | } 89 | 90 | // resize the deque 91 | output.resize(input.size() + initPoint); 92 | 93 | // Advances the iterator it by initPoint positions 94 | typename std::deque::iterator it = output.begin(); 95 | std::advance(it, initPoint); 96 | 97 | // copy the vector into the deque from the initPoint position 98 | std::copy(input.begin(), input.end(), it); 99 | 100 | return true; 101 | } 102 | -------------------------------------------------------------------------------- /devices/wholeBodyDynamics/test/ergocub_simulated_wholebodydynamics.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | (torso_pitch,torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll) 7 | ((camera_tilt,0.17), (l_pinkie_prox,-0.17), (r_pinkie_prox,-0.17)) 8 | model.urdf 9 | (0,0,-9.81) 10 | (l_hand_palm,r_hand_palm,root_link,l_foot_front,l_foot_rear,r_foot_front,r_foot_rear,l_upper_leg,r_upper_leg) 11 | waist_imu_0 12 | true 13 | 2 14 | true 15 | false 16 | false 17 | true 18 | 3.0 19 | 3.0 20 | 3.0 21 | 3.0 22 | 0.002 23 | false 24 | true 25 | 26 | 27 | waist_imu_0 28 | waist_imu_0 29 | 30 | 31 | 32 | (l_leg_ft, l_foot_front_ft, l_foot_rear_ft, r_leg_ft, r_foot_front_ft, r_foot_rear_ft, r_arm_ft, l_arm_ft) 33 | 34 | 35 | 36 | true 37 | root_link 38 | (torso_pitch,torso_roll,torso_yaw,neck_pitch,neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll) 39 | 40 | 41 | 42 | 43 | 44 | simulated_controlboard 45 | 46 | 47 | simulated_imu 48 | 49 | 50 | simulated_fts 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /idl/floatingBaseEstimatorRPC/codyco/floatingBaseEstimatorRPC.thrift: -------------------------------------------------------------------------------- 1 | #floatingBaseEstimator_IDLServer.thrift 2 | namespace yarp codyco 3 | 4 | /** 5 | * Structure representing an homogeneous transform, i.e. 6 | * a 4x4 matrix with structure: 7 | * 8 | * B_H_C = ( xx xy xz x ) 9 | * ( yx yy yz y ) 10 | * ( zx zy zz z ) 11 | * ( 0 0 0 1 ) 12 | * 13 | * A matrix like that respresents the location of a frame C w.r.t. to a frame B. 14 | * 15 | * For more information on the semantics of such transformation, please 16 | * refer to http://repository.tue.nl/849895 . 17 | */ 18 | struct HomTransform 19 | { 20 | 1: double x; /* x linear position [m] */ 21 | 2: double y; /* y linear position [m] */ 22 | 3: double z; /* z linear position [m] */ 23 | 4: double xx; /* xx element of the rotation matrix */ 24 | 5: double xy; /* xy element of the rotation matrix */ 25 | 6: double xz; /* xz element of the rotation matrix */ 26 | 7: double yx; /* yx element of the rotation matrix */ 27 | 8: double yy; /* yy element of the rotation matrix */ 28 | 9: double yz; /* yz element of the rotation matrix */ 29 | 10: double zx; /* zx element of the rotation matrix */ 30 | 11: double zy; /* zy element of the rotation matrix */ 31 | 12: double zz; /* zz element of the rotation matrix */ 32 | } 33 | 34 | /** 35 | * floatingBaseEstimatorRPC 36 | * 37 | * Interface. 38 | */ 39 | service floatingBaseEstimatorRPC 40 | { 41 | /** 42 | * Reset the odometry world to be (initially) a frame specified in the robot model, 43 | * and specify a frame that is assumed to be fixed in the odometry. 44 | 45 | * @param initial_world_frame the frame of the robot model that is assume to be initially 46 | * coincident with the world/inertial frame. 47 | * @param new_fixed_frame the name of a frame attached to the link that should be considered fixed 48 | * @return true/false on success/failure (typically if the frame/link names are wrong) 49 | */ 50 | bool resetSimpleLeggedOdometry(1:string initial_world_frame, 2:string initial_fixed_frame); 51 | 52 | /** 53 | * Reset the odometry world to be (initially) a frame specified in the robot model, 54 | * and specify a frame that is assumed to be fixed in the odometry. 55 | 56 | * @param initial_reference_frame the frame of the robot model with respect to which we expressed the location of the world. 57 | * @param initial_reference_frame_H_world the initial location of the world w.r.t. the initial_reference_frame. 58 | * @param new_fixed_frame the name of a frame attached to the link that should be considered fixed. 59 | * @return true/false on success/failure (typically if the frame/link names are wrong) 60 | */ 61 | bool resetSimpleLeggedOdometryToArbitraryFrame(1:string initial_reference_frame, 2:HomTransform initial_reference_frame_H_world, 3:string initial_fixed_frame); 62 | 63 | /** 64 | * Change the link that is considered fixed by the odometry. 65 | * @param new_fixed_frame the name of a frame attached to the link that should be considered fixed 66 | * @return true/false on success/failure (typically if the frame names are wrong) 67 | */ 68 | bool changeFixedLinkSimpleLeggedOdometry(1:string new_fixed_frame); 69 | 70 | /** 71 | * Get the current settings in the form of a string. 72 | * @return the current settings as a human readable string. 73 | */ 74 | string getCurrentSettingsString(); 75 | } 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /devices/wholeBodyDynamics/test/fakeFTs/fakeFTs.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) 3 | * SPDX-License-Identifier: BSD-3-Clause 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | /** 14 | * @ingroup dev_impl_fake 15 | * \brief `fakeFTs` : fake device implementing the device interface typically implemented by an Inertial Measurement Unit 16 | * 17 | * | YARP device name | 18 | * |:-----------------:| 19 | * | `fakeFTs` | 20 | * 21 | * 22 | * The parameters accepted by this device are: 23 | * | Parameter name | SubParameter | Type | Units | Default Value | Required | Description | Notes | 24 | * |:--------------:|:--------------:|:-------:|:--------------:|:-------------:|:--------------------------: |:-----------------------------------------------------------------:|:-----:| 25 | * | period | - | int | millisecond | 10 | No | Period over which the measurement is updated. | | 26 | * | sensorNames | - | int | millisecond | 10 | No | Name of sensor names. | | 27 | * 28 | */ 29 | class fakeFTs : 30 | public yarp::dev::DeviceDriver, 31 | public yarp::os::PeriodicThread, 32 | public yarp::dev::ITemperatureSensors, 33 | public yarp::dev::ISixAxisForceTorqueSensors 34 | { 35 | public: 36 | fakeFTs(); 37 | fakeFTs(const fakeFTs&) = delete; 38 | fakeFTs(fakeFTs&&) = delete; 39 | fakeFTs& operator=(const fakeFTs&) = delete; 40 | fakeFTs& operator=(fakeFTs&&) = delete; 41 | 42 | ~fakeFTs() override; 43 | 44 | // Device Driver interface 45 | bool open(yarp::os::Searchable &config) override; 46 | bool close() override; 47 | 48 | // ITemperatureSensors 49 | virtual size_t getNrOfTemperatureSensors() const override; 50 | virtual yarp::dev::MAS_status getTemperatureSensorStatus(size_t sens_index) const override; 51 | virtual bool getTemperatureSensorName(size_t sens_index, std::string &name) const override; 52 | virtual bool getTemperatureSensorFrameName(size_t sens_index, std::string &frameName) const override; 53 | virtual bool getTemperatureSensorMeasure(size_t sens_index, double& out, double& timestamp) const override; 54 | virtual bool getTemperatureSensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const override; 55 | 56 | // ISixAxisForceTorqueSensors 57 | virtual size_t getNrOfSixAxisForceTorqueSensors() const override; 58 | virtual yarp::dev::MAS_status getSixAxisForceTorqueSensorStatus(size_t sens_index) const override; 59 | virtual bool getSixAxisForceTorqueSensorName(size_t sens_index, std::string &name) const override; 60 | virtual bool getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string &frameName) const override; 61 | virtual bool getSixAxisForceTorqueSensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const override; 62 | 63 | private: 64 | size_t genericGetNrOfSensors() const; 65 | yarp::dev::MAS_status genericGetStatus(size_t sens_index) const; 66 | bool genericGetSensorName(size_t sens_index, std::string &name) const; 67 | bool genericGetFrameName(size_t sens_index, std::string &frameName) const; 68 | 69 | bool threadInit() override; 70 | void run() override; 71 | 72 | double dummy_value; 73 | yarp::os::Stamp lastStamp; 74 | std::vector m_sensorNames; 75 | 76 | bool constantValue; 77 | }; 78 | -------------------------------------------------------------------------------- /devices/baseEstimatorV1/app/robots/iCubGenova04/launch-fbe-analogsens.xml: -------------------------------------------------------------------------------- 1 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | ("/icub/head", "/icub/torso", "/icub/left_arm", "/icub/right_arm", "/icub/left_leg", "/icub/right_leg") 15 | ("neck_pitch", "neck_roll", "neck_yaw", "torso_pitch", "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") 16 | /baseestimation 17 | 18 | 19 | 20 | 21 | /icub/inertial 22 | /baseestimation/head/imu:i 23 | 24 | 25 | 26 | 32 | 33 | 34 | 35 | /icub/left_arm/analog:o 36 | /baseestimation/l_arm_ft 37 | 38 | 39 | 40 | /icub/right_arm/analog:o 41 | /baseestimation/r_arm_ft 42 | 43 | 44 | 45 | /icub/left_leg/analog:o 46 | /baseestimation/l_leg_ft 47 | 48 | 49 | 50 | /icub/right_leg/analog:o 51 | /baseestimation/r_leg_ft 52 | 53 | 54 | 55 | /icub/left_foot/analog:o 56 | /baseestimation/l_foot_ft:i 57 | 58 | 59 | 60 | /icub/right_foot/analog:o 61 | /baseestimation/r_foot_ft:i 62 | 63 | 64 | 65 | 66 | 67 | true 68 | true 69 | 70 | 0.2 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/launch-fbe-analogsens.xml: -------------------------------------------------------------------------------- 1 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | ("/icubSim/head", "/icubSim/torso", "/icubSim/left_arm", "/icubSim/right_arm", "/icubSim/left_leg", "/icubSim/right_leg") 15 | ("neck_pitch", "neck_roll", "neck_yaw", "torso_pitch", "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") 16 | /baseestimation 17 | 18 | 19 | 20 | 21 | /icubSim/inertial 22 | /baseestimation/head/imu:i 23 | 24 | 25 | 26 | 32 | 33 | 34 | 35 | /icubSim/left_arm/analog:o 36 | /baseestimation/l_arm_ft 37 | 38 | 39 | 40 | /icubSim/right_arm/analog:o 41 | /baseestimation/r_arm_ft 42 | 43 | 44 | 45 | /icubSim/left_leg/analog:o 46 | /baseestimation/l_leg_ft 47 | 48 | 49 | 50 | /icubSim/right_leg/analog:o 51 | /baseestimation/r_leg_ft 52 | 53 | 54 | 55 | /icubSim/left_foot/analog:o 56 | /baseestimation/l_foot_ft:i 57 | 58 | 59 | 60 | /icubSim/right_foot/analog:o 61 | /baseestimation/r_foot_ft:i 62 | 63 | 64 | 65 | 66 | 67 | true 68 | true 69 | 70 | 0.2 71 | 72 | 73 | 74 | 75 | 76 | 77 | -------------------------------------------------------------------------------- /devices/wholeBodyDynamics/SixAxisForceTorqueMeasureHelpers.h: -------------------------------------------------------------------------------- 1 | #ifndef SIX_AXIS_FORCE_TORQUE_MEASURE_PROCESSOR_H 2 | #define SIX_AXIS_FORCE_TORQUE_MEASURE_PROCESSOR_H 3 | 4 | // iDynTree includes 5 | #include 6 | #include 7 | 8 | 9 | namespace wholeBodyDynamics 10 | { 11 | 12 | /** 13 | * Class for processing the raw measure of 14 | * a Six Axis Force Torque sensor. 15 | * 16 | * In particular this class process the raw measurement of a 17 | * Six Axis FT sensor using an affine function: 18 | * 19 | * ftNew = secondaryCalibrationMatrix*ftOld - offset 20 | * 21 | * Where offset is the offset of the F/T sensor (tipically computed 22 | * online at given instants where only one external force is known 23 | * to be acting on the robot, while secondaryCalibrationMatrix is a 24 | * secondary calibration matrix, that is tipically set to the identity. 25 | * 26 | */ 27 | class SixAxisForceTorqueMeasureProcessor 28 | { 29 | private: 30 | iDynTree::Matrix6x6 m_secondaryCalibrationMatrix; 31 | iDynTree::Wrench m_offset; 32 | iDynTree::Wrench m_estimated_offset; 33 | iDynTree::Vector6 m_temperatureCoefficients; 34 | double m_tempOffset; 35 | 36 | public: 37 | /** 38 | * Default constructor: the secondaryCalibrationMatrix is 39 | * set to the identity, while the offset is set to 0. 40 | */ 41 | SixAxisForceTorqueMeasureProcessor(); 42 | 43 | /** 44 | * Accessor to the offset. 45 | */ 46 | iDynTree::Wrench & offset(); 47 | 48 | /** 49 | * Const accessor to the offset. 50 | */ 51 | const iDynTree::Wrench & offset() const; 52 | 53 | /** 54 | * Accessor to the offline estimated offset. 55 | */ 56 | iDynTree::Wrench & estimatedOffset(); 57 | 58 | /** 59 | * Const accessor to the offline estimated offset. 60 | */ 61 | const iDynTree::Wrench & estimatedOffset() const; 62 | 63 | /** 64 | * Accessor to the secondary calibration matrix. 65 | */ 66 | iDynTree::Matrix6x6 & secondaryCalibrationMatrix(); 67 | 68 | /** 69 | * Const accessor to the secondary calibration matrix. 70 | */ 71 | const iDynTree::Matrix6x6 & secondaryCalibrationMatrix() const; 72 | 73 | /** 74 | * Accessor to the temperature offset. 75 | */ 76 | double & tempOffset(); 77 | 78 | /** 79 | * Const accessor to the temperature offset. 80 | */ 81 | const double & tempOffset() const; 82 | 83 | /** 84 | * Accessor to the temperature Coefficients. 85 | */ 86 | iDynTree::Vector6 & temperatureCoefficients(); 87 | 88 | /** 89 | * Const accessor to the temperature Coefficients. 90 | */ 91 | const iDynTree::Vector6 & temperatureCoefficients() const; 92 | 93 | /** 94 | * Process the input F/T. 95 | * 96 | * Returns secondaryCalibrationMatrix*input + offset . 97 | */ 98 | iDynTree::Wrench filt(const iDynTree::Wrench & input) const; 99 | 100 | /** 101 | * Process the input F/T. 102 | * 103 | * Returns secondaryCalibrationMatrix*input + offset + temperatureCoefficients*(temperature-temperatureOffset). 104 | */ 105 | iDynTree::Wrench filt(const iDynTree::Wrench & input, const double & temperature) const; 106 | 107 | /** 108 | * Process the input F/T by only applyng the calibration matrix. 109 | */ 110 | iDynTree::Wrench applySecondaryCalibrationMatrix(const iDynTree::Wrench & input) const; 111 | 112 | /** 113 | * Process the input F/T by only applyng the calibration matrix and temperature coefficients. 114 | */ 115 | iDynTree::Wrench applySecondaryCalibrationMatrix(const iDynTree::Wrench & input, const double & temperature) const; 116 | 117 | 118 | 119 | }; 120 | 121 | } 122 | 123 | #endif 124 | -------------------------------------------------------------------------------- /devices/baseEstimatorV1/scope/waist_imu_scope.xml: -------------------------------------------------------------------------------- 1 | 8 | 9 | 10 | 19 | 20 | 26 | 27 | 33 | 39 | 40 | 49 | 50 | 56 | 57 | 63 | 69 | 70 | 71 | 80 | 81 | 87 | 88 | 94 | 100 | 101 | 102 | 103 | 104 | -------------------------------------------------------------------------------- /devices/genericSensorClient/GenericSensorClient.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016 Fondazione Istituto Italiano di Tecnologia 3 | * Author: Silvio Traversaro 4 | * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT 5 | */ 6 | 7 | #ifndef YARP_DEV_GENERICSENSORCLIENT_H 8 | #define YARP_DEV_GENERICSENSORCLIENT_H 9 | 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | /** 24 | * Class copied from the InputPortProcessor class in AnalogSensorClient. 25 | * Once we port this in YARP we can merge this two classes. 26 | */ 27 | class GSInputPortProcessor : public yarp::os::BufferedPort 28 | { 29 | std::mutex mutex; 30 | yarp::sig::Vector lastVector; 31 | yarp::os::Stamp lastStamp; 32 | double deltaT; 33 | double deltaTMax; 34 | double deltaTMin; 35 | double prev; 36 | double now; 37 | 38 | bool dataAvailable; 39 | 40 | int count; 41 | 42 | public: 43 | inline void resetStat(); 44 | 45 | GSInputPortProcessor(); 46 | 47 | using yarp::os::BufferedPort::onRead; 48 | virtual void onRead(yarp::sig::Vector &v); 49 | 50 | inline bool getLast(yarp::sig::Vector &data, yarp::os::Stamp &stmp); 51 | 52 | inline int getIterations(); 53 | 54 | // time is in ms 55 | void getEstFrequency(int &ite, double &av, double &min, double &max); 56 | 57 | bool getState(); 58 | 59 | int getChannels(); 60 | }; 61 | 62 | 63 | namespace yarp { 64 | namespace dev { 65 | 66 | /** 67 | * @ingroup dev_impl_wrapper 68 | * 69 | * \brief Device that reads a sensor exposing a IGenericSensor interface from the YARP network. 70 | * 71 | * This device will connect to a port opened by the ServerInertial device and read the data broadcasted 72 | * making them available to use for the user application. 73 | * 74 | * Parameters accepted in the config argument of the open method: 75 | * | Parameter name | Type | Units | Default Value | Required | Description | Notes | 76 | * |:--------------:|:------:|:-----:|:-------------:|:--------: |:-------------:|:-----:| 77 | * | local | string | | | Yes | full name if the port opened by the device | must start with a '/' character | 78 | * | remote | string | | | Yes | full name of the port the device need to connect to | must start with a '/' character | 79 | * | carrier | string | | fast_tcp | No | type of carrier to use, like fast_tcp, tcp, udp and so on ... | - | 80 | * 81 | * The device will create a port with name and will connect to a port colled at startup, 82 | * ex: /myModule/linertial , and will connect to a port called /icub/inertial. 83 | * 84 | **/ 85 | class GenericSensorClient: public yarp::dev::DeviceDriver, 86 | public yarp::dev::IPreciselyTimed, 87 | public yarp::dev::IGenericSensor 88 | { 89 | protected: 90 | GSInputPortProcessor inputPort; 91 | std::string local; 92 | std::string remote; 93 | yarp::os::Stamp lastTs; //used by IPreciselyTimed 94 | 95 | void removeLeadingTrailingSlashesOnly(std::string &name); 96 | 97 | public: 98 | 99 | /* DeviceDriver methods */ 100 | bool open(yarp::os::Searchable& config); 101 | bool close(); 102 | 103 | /* IGenericSensor methods */ 104 | virtual bool read(yarp::sig::Vector &out); 105 | virtual bool getChannels(int *nc); 106 | 107 | /** 108 | * Not implemeted in GenericSensorClient 109 | */ 110 | virtual bool calibrate(int ch, double v); 111 | 112 | /* IPreciselyTimed methods */ 113 | yarp::os::Stamp getLastInputStamp(); 114 | }; 115 | 116 | } 117 | 118 | } 119 | 120 | #endif // YARP_DEV_GENERICSENSORCLIENT_H 121 | -------------------------------------------------------------------------------- /devices/wholeBodyDynamics/app/launch-wholebodydynamics-icub-six-fts-sim.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | /icubSim/torso 8 | /wholeBodyDynamics/torso 9 | 10 | 11 | 12 | /icubSim/left_arm 13 | /wholeBodyDynamics/left_arm 14 | 15 | 16 | 17 | /icubSim/right_arm 18 | /wholeBodyDynamics/right_arm 19 | 20 | 21 | 22 | /icubSim/left_leg 23 | /wholeBodyDynamics/left_leg 24 | 25 | 26 | 27 | /icubSim/right_leg 28 | /wholeBodyDynamics/right_leg 29 | 30 | 31 | 32 | /icubSim/head 33 | /wholeBodyDynamics/head 34 | 35 | 36 | 58 | 59 | 60 | 61 | /icubSim/head/inertials 62 | /wholeBodyDynamics/imu 63 | 64 | 65 | 66 | 67 | /icubSim/left_arm/FT 68 | /wholeBodyDynamics/l_arm_ft 69 | 70 | 71 | 72 | /icubSim/right_arm/FT 73 | /wholeBodyDynamics/r_arm_ft 74 | 75 | 76 | 77 | /icubSim/left_leg/FT 78 | /wholeBodyDynamics/l_leg_ft 79 | 80 | 81 | 82 | /icubSim/right_leg/FT 83 | /wholeBodyDynamics/r_leg_ft 84 | 85 | 86 | 87 | 88 | 89 | -------------------------------------------------------------------------------- /cmake/AddUninstallTarget.cmake: -------------------------------------------------------------------------------- 1 | #.rst: 2 | # AddUninstallTarget 3 | # ------------------ 4 | # 5 | # Add the "uninstall" target for your project:: 6 | # 7 | # include(AddUninstallTarget) 8 | # 9 | # 10 | # will create a file ``cmake_uninstall.cmake`` in the build directory and add a 11 | # custom target ``uninstall`` (or ``UNINSTALL`` on Visual Studio and Xcode) that 12 | # will remove the files installed by your package (using 13 | # ``install_manifest.txt``). 14 | # See also 15 | # https://gitlab.kitware.com/cmake/community/wikis/FAQ#can-i-do-make-uninstall-with-cmake 16 | # 17 | # The :module:`AddUninstallTarget` module must be included in your main 18 | # ``CMakeLists.txt``. If included in a subdirectory it does nothing. 19 | # This allows you to use it safely in your main ``CMakeLists.txt`` and include 20 | # your project using ``add_subdirectory`` (for example when using it with 21 | # :cmake:module:`FetchContent`). 22 | # 23 | # If the ``uninstall`` target already exists, the module does nothing. 24 | 25 | #============================================================================= 26 | # Copyright 2008-2013 Kitware, Inc. 27 | # Copyright 2013 Istituto Italiano di Tecnologia (IIT) 28 | # Authors: Daniele E. Domenichelli 29 | # 30 | # Distributed under the OSI-approved BSD License (the "License"); 31 | # see accompanying file Copyright.txt for details. 32 | # 33 | # This software is distributed WITHOUT ANY WARRANTY; without even the 34 | # implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 35 | # See the License for more information. 36 | #============================================================================= 37 | # (To distribute this file outside of CMake, substitute the full 38 | # License text for the above reference.) 39 | 40 | 41 | # AddUninstallTarget works only when included in the main CMakeLists.txt 42 | if(NOT "${CMAKE_CURRENT_BINARY_DIR}" STREQUAL "${CMAKE_BINARY_DIR}") 43 | return() 44 | endif() 45 | 46 | # The name of the target is uppercase in MSVC and Xcode (for coherence with the 47 | # other standard targets) 48 | if("${CMAKE_GENERATOR}" MATCHES "^(Visual Studio|Xcode)") 49 | set(_uninstall "UNINSTALL") 50 | else() 51 | set(_uninstall "uninstall") 52 | endif() 53 | 54 | # If target is already defined don't do anything 55 | if(TARGET ${_uninstall}) 56 | return() 57 | endif() 58 | 59 | 60 | set(_filename cmake_uninstall.cmake) 61 | 62 | file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/${_filename}" 63 | "if(NOT EXISTS \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\") 64 | message(WARNING \"Cannot find install manifest: \\\"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\\\"\") 65 | return() 66 | endif() 67 | file(READ \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\" files) 68 | string(STRIP \"${files}\" files) 69 | string(REGEX REPLACE \"\\n\" \";\" files \"${files}\") 70 | list(REVERSE files) 71 | foreach(file ${files}) 72 | if(IS_SYMLINK \"$ENV{DESTDIR}${file}\" OR EXISTS \"$ENV{DESTDIR}${file}\") 73 | message(STATUS \"Uninstalling: $ENV{DESTDIR}${file}\") 74 | execute_process( 75 | COMMAND ${CMAKE_COMMAND} -E remove \"$ENV{DESTDIR}${file}\" 76 | OUTPUT_VARIABLE rm_out 77 | RESULT_VARIABLE rm_retval) 78 | if(NOT \"${rm_retval}\" EQUAL 0) 79 | message(FATAL_ERROR \"Problem when removing \\\"$ENV{DESTDIR}${file}\\\"\") 80 | endif() 81 | else() 82 | message(STATUS \"Not-found: $ENV{DESTDIR}${file}\") 83 | endif() 84 | endforeach(file) 85 | ") 86 | 87 | set(_desc "Uninstall the project...") 88 | if(CMAKE_GENERATOR STREQUAL "Unix Makefiles") 89 | set(_comment COMMAND $\(CMAKE_COMMAND\) -E cmake_echo_color --switch=$\(COLOR\) --cyan "${_desc}") 90 | else() 91 | set(_comment COMMENT "${_desc}") 92 | endif() 93 | add_custom_target(${_uninstall} 94 | ${_comment} 95 | COMMAND ${CMAKE_COMMAND} -P ${_filename} 96 | USES_TERMINAL 97 | BYPRODUCTS uninstall_byproduct 98 | WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}") 99 | set_property(SOURCE uninstall_byproduct PROPERTY SYMBOLIC 1) 100 | 101 | set_property(TARGET ${_uninstall} PROPERTY FOLDER "CMakePredefinedTargets") 102 | 103 | -------------------------------------------------------------------------------- /devices/virtualAnalogClient/VirtualAnalogClient.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016 Fondazione Istituto Italiano di Tecnologia 3 | * Author: Silvio Traversaro 4 | * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT 5 | */ 6 | 7 | #ifndef YARP_DEV_VIRTUALANALOGCLIENT_H 8 | #define YARP_DEV_VIRTUALANALOGCLIENT_H 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | namespace yarp { 25 | namespace dev { 26 | 27 | /** 28 | * @ingroup dev_impl_wrapper 29 | * 30 | * \brief Device that connects to a virtualAnalogServer to publish torque estimates. 31 | * 32 | * \section VirtualAnalogClient Description of input parameters 33 | * 34 | * This device will connect to a virtualAnalogServer to publish torque estimates. 35 | * 36 | * Parameters accepted in the config argument of the open method: 37 | * | Parameter name | Type | Units | Default Value | Required | Description | Notes | 38 | * |:--------------:|:------:|:-----:|:-------------:|:--------: |:-------------:|:-----:| 39 | * | local | string | | | Yes | full name if the port opened by the device | must start with a '/' character | 40 | * | remote | string | | | Yes | full name of the port the device need to connect to | must start with a '/' character | 41 | * | carrier | string | | tcp | No | type of carrier to use, like tcp, udp and so on ... | - | 42 | * | AxisName | vector of strings | - | - | Yes | name of the axes in which the torque estimate is published | - | 43 | * | AxisType | vector of strings | - |revolute| No | type of the axies in which the torque estimate is published | - | 44 | * | virtualAnalogSensorInteger | int | - | - | Yes | A virtualAnalogServer specific integer, check the VirtualAnalogServer for more info. | - | 45 | * | autoconnect | bool | - | true | No | Specify if port should be connected or not | - | 46 | * 47 | * The device will create a port with name and will connect to a port colled at startup, 48 | * ex: /wholeBodyDynamics/left_leg/Torques:o , and will connect to a port called /icub/joint_vsens/left_leg:i . 49 | * 50 | * A new message is written on this YARP port as soon as the updateMeasure is called (so the multiaxis updateMeasure should 51 | * always be prefered). 52 | * 53 | * For the single axis updateMeasure, the value sent for the not-update axis will be the one stored in a buffer, that is initialized to zero. 54 | * 55 | **/ 56 | class VirtualAnalogClient: public DeviceDriver, 57 | public IVirtualAnalogSensor, 58 | public IAxisInfo 59 | { 60 | protected: 61 | std::string m_local; 62 | std::string m_remote; 63 | int m_virtualAnalogSensorInteger; 64 | 65 | std::vector m_axisName; 66 | std::vector m_axisType; 67 | 68 | yarp::os::BufferedPort m_outputPort; 69 | 70 | yarp::sig::Vector measureBuffer; 71 | 72 | /** 73 | * Publish the data contained in the measureBuffer on the port. 74 | */ 75 | void sendData(); 76 | 77 | public: 78 | VirtualAnalogClient(); 79 | virtual ~VirtualAnalogClient(); 80 | 81 | /* DevideDriver methods (documented in DeviceDriver class) */ 82 | virtual bool open(yarp::os::Searchable& config); 83 | virtual bool close(); 84 | 85 | /* IVirtualAnalogSensor methods (documented in IVirtualAnalogSensor class) */ 86 | virtual VAS_status getVirtualAnalogSensorStatus(int ch); 87 | virtual int getVirtualAnalogSensorChannels(); 88 | virtual bool updateVirtualAnalogSensorMeasure(yarp::sig::Vector &measure); 89 | virtual bool updateVirtualAnalogSensorMeasure(int ch, double &measure); 90 | 91 | /** IAxisInfo methods (documented in IVirtualAnalogSensor class) */ 92 | virtual bool getAxisName(int axis, std::string& name); 93 | virtual bool getJointType(int axis, yarp::dev::JointTypeEnum& type); 94 | virtual bool getAxes(int* ax); 95 | }; 96 | 97 | } 98 | } 99 | 100 | #endif // YARP_DEV_VIRTUALANALOGCLIENT_H 101 | -------------------------------------------------------------------------------- /devices/wholeBodyDynamics/GravityCompensationHelpers.h: -------------------------------------------------------------------------------- 1 | #ifndef GRAVITY_COMPENSATION_HELPERS_H 2 | #define GRAVITY_COMPENSATION_HELPERS_H 3 | 4 | // iDynTree includes 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | namespace wholeBodyDynamics 11 | { 12 | 13 | /** 14 | * Class computing the gravity compensation torques 15 | * using the same accelerometers measurement used by 16 | * wholeBodyDynamics. 17 | * 18 | * NOTE : the estimation of the gravity disregards 19 | * as negligible the non-gravitational acceleration 20 | * measured by the IMU accelerometer. 21 | * 22 | */ 23 | class GravityCompensationHelper 24 | { 25 | private: 26 | iDynTree::Model m_model; 27 | bool m_isModelValid; 28 | bool m_isKinematicsUpdated; 29 | 30 | /**< Traveral used for the dynamics computations */ 31 | iDynTree::Traversal m_dynamicTraversal; 32 | 33 | /** 34 | * Vector of Traversal used for the kinematic computations. 35 | * m_kinematicTraversals[l] contains the traversal with base link l . 36 | */ 37 | std::vector m_kinematicTraversals; 38 | 39 | /** 40 | * Helper functions for dealing with the kinematic traversal dynamic allocation 41 | */ 42 | void allocKinematicTraversals(const size_t nrOfLinks); 43 | void freeKinematicTraversals(); 44 | 45 | iDynTree::JointPosDoubleArray m_jointPos; 46 | iDynTree::JointDOFsDoubleArray m_jointDofsZero; 47 | iDynTree::LinkVelArray m_linkVels; 48 | iDynTree::LinkAccArray m_linkProperAccs; 49 | iDynTree::LinkNetExternalWrenches m_linkNetExternalWrenchesZero; 50 | iDynTree::LinkInternalWrenches m_linkIntWrenches; 51 | iDynTree::FreeFloatingGeneralizedTorques m_generalizedTorques; 52 | 53 | public: 54 | /** 55 | * Default constructor 56 | */ 57 | GravityCompensationHelper(); 58 | 59 | ~GravityCompensationHelper(); 60 | 61 | /** 62 | * Load model 63 | */ 64 | bool loadModel(const iDynTree::Model & _model , const std::string dynamicBase); 65 | 66 | /** 67 | * Set the kinematic information necessary for the gravity torques estimation using the 68 | * proper acceleration coming from an acceleromter. 69 | * 70 | * NOTE : the estimation of the gravity disregards 71 | * as negligible the non-gravitational acceleration 72 | * measured by the IMU accelerometer. 73 | * 74 | * @param[in] jointPos the position of the joints of the model. 75 | * @param[in] floatingFrame the index of the frame for which proper acceleration is provided. 76 | * @param[in] properClassicalLinearAcceleration proper (actual acceleration-gravity) classical acceleration 77 | * of the origin of the specified frame, 78 | * expressed in the specified frame orientation. 79 | * @return true if all went ok, false otherwise. 80 | */ 81 | bool updateKinematicsFromProperAcceleration(const iDynTree::JointPosDoubleArray & jointPos, 82 | const iDynTree::FrameIndex & floatingFrame, 83 | const iDynTree::Vector3 & properClassicalLinearAcceleration); 84 | 85 | /** 86 | * Set the kinematic information necessary for the gravity torques estimation using the 87 | * assumed known gravity vector on frame. 88 | * 89 | * \note This is implemented as updateKinematicsFromProperAcceleration(jointPos,floatingFrame,-gravity); 90 | * 91 | * @param[in] jointPos the position of the joints of the model. 92 | * @param[in] floatingFrame the index of the frame for which proper acceleration is provided. 93 | * @param[in] properClassicalLinearAcceleration gravity acceleration 94 | * ot the origin of the specified frame, 95 | * expressed in the specified frame orientation. 96 | * @return true if all went ok, false otherwise. 97 | */ 98 | bool updateKinematicsFromGravity(const iDynTree::JointPosDoubleArray & jointPos, 99 | const iDynTree::FrameIndex & floatingFrame, 100 | const iDynTree::Vector3 & gravity); 101 | 102 | 103 | /** 104 | * Get the gravity compensation torques. 105 | */ 106 | bool getGravityCompensationTorques(iDynTree::JointDOFsDoubleArray & jointTrqs); 107 | 108 | }; 109 | 110 | } 111 | 112 | #endif 113 | -------------------------------------------------------------------------------- /devices/genericSensorClient/GenericSensorClient.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2010 RobotCub Consortium 3 | * Author: Lorenzo Natale 4 | * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT 5 | * 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | const int GS_ANALOG_TIMEOUT=100; //ms 15 | 16 | using namespace yarp::os; 17 | 18 | inline void GSInputPortProcessor::resetStat() 19 | { 20 | std::lock_guard guard(mutex); 21 | 22 | dataAvailable = false; 23 | count=0; 24 | deltaT=0; 25 | deltaTMax=0; 26 | deltaTMin=1e22; 27 | now=Time::now(); 28 | prev=now; 29 | } 30 | 31 | GSInputPortProcessor::GSInputPortProcessor() 32 | { 33 | resetStat(); 34 | } 35 | 36 | void GSInputPortProcessor::onRead(yarp::sig::Vector &v) 37 | { 38 | now=Time::now(); 39 | 40 | std::lock_guard guard(mutex); 41 | 42 | if (count>0) 43 | { 44 | double tmpDT=now-prev; 45 | 46 | deltaT+=tmpDT; 47 | 48 | if (tmpDT>deltaTMax) 49 | { 50 | deltaTMax=tmpDT; 51 | } 52 | 53 | if (tmpDT guard(mutex); 85 | 86 | if (dataAvailable) 87 | { 88 | data=lastVector; 89 | stmp = lastStamp; 90 | } 91 | 92 | return dataAvailable; 93 | } 94 | 95 | inline int GSInputPortProcessor::getIterations() 96 | { 97 | std::lock_guard guard(mutex); 98 | 99 | int ret=count; 100 | return ret; 101 | } 102 | 103 | // time is in ms 104 | void GSInputPortProcessor::getEstFrequency(int &ite, double &av, double &min, double &max) 105 | { 106 | std::lock_guard guard(mutex); 107 | 108 | ite=count; 109 | min=deltaTMin*1000; 110 | max=deltaTMax*1000; 111 | if (count<1) 112 | { 113 | av=0; 114 | } 115 | else 116 | { 117 | av=deltaT/count; 118 | } 119 | av=av*1000; 120 | } 121 | 122 | bool GSInputPortProcessor::getState() 123 | { 124 | return dataAvailable; 125 | } 126 | 127 | int GSInputPortProcessor::getChannels() 128 | { 129 | return (int)lastVector.size(); 130 | } 131 | 132 | 133 | bool yarp::dev::GenericSensorClient::open(yarp::os::Searchable &config) 134 | { 135 | std::string carrier = config.check("carrier", Value("fast_tcp"), "default carrier for streaming robot state").asString().c_str(); 136 | 137 | local.clear(); 138 | remote.clear(); 139 | 140 | local = config.find("local").asString().c_str(); 141 | remote = config.find("remote").asString().c_str(); 142 | 143 | if (local=="") 144 | { 145 | yError("GenericSensorClient::open() error you have to provide valid local name"); 146 | return false; 147 | } 148 | if (remote=="") 149 | { 150 | yError("GenericSensorClient::open() error you have to provide valid remote name\n"); 151 | return false; 152 | } 153 | 154 | if (!inputPort.open(local.c_str())) 155 | { 156 | yError("GenericSensorClient::open() error could not open port %s, check network", local.c_str()); 157 | return false; 158 | } 159 | inputPort.useCallback(); 160 | 161 | bool ok=Network::connect(remote.c_str(), local.c_str(), carrier.c_str()); 162 | if (!ok) 163 | { 164 | yError("GenericSensorClient::open() error could not connect to %s", remote.c_str()); 165 | return false; 166 | } 167 | 168 | return true; 169 | } 170 | 171 | bool yarp::dev::GenericSensorClient::close() 172 | { 173 | inputPort.close(); 174 | return true; 175 | } 176 | 177 | bool yarp::dev::GenericSensorClient::read(yarp::sig::Vector &out) 178 | { 179 | return inputPort.getLast(out, lastTs); 180 | } 181 | 182 | bool yarp::dev::GenericSensorClient::calibrate(int /*ch*/, double /*v*/) 183 | { 184 | return false; 185 | } 186 | 187 | bool yarp::dev::GenericSensorClient::getChannels(int* nc) 188 | { 189 | bool ret = inputPort.getState(); 190 | 191 | *nc = inputPort.getChannels(); 192 | 193 | return ret; 194 | } 195 | 196 | Stamp yarp::dev::GenericSensorClient::getLastInputStamp() 197 | { 198 | return lastTs; 199 | } 200 | -------------------------------------------------------------------------------- /devices/wholeBodyDynamics/app/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: (C) 2017 Fondazione Istituto Italiano di Tecnologia 2 | # Authors: Silvio Traversaro 3 | # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT 4 | 5 | include(CMakeParseArguments) 6 | 7 | # Macro for installing configuration files 8 | macro(install_wbd_robots_files) 9 | set(_options NO_ROBOT) 10 | set(_oneValueArgs YARP_ROBOT_NAME) 11 | set(_multiValueArgs EXTERNAL_WBD_CONF ROBOT_WBD_CONF EXTERNAL_YRI_CONF) 12 | cmake_parse_arguments(WBD "${_options}" "${_oneValueArgs}" "${_multiValueArgs}" ${ARGN} ) 13 | 14 | # Configure and install files for using wholebodydynamics in the robot's yarprobotinterface 15 | if(NOT ${WBD_NO_ROBOT}) 16 | configure_file(${CMAKE_CURRENT_SOURCE_DIR}/${WBD_ROBOT_WBD_CONF} 17 | ${CMAKE_CURRENT_BINARY_DIR}/${WBD_YARP_ROBOT_NAME}/wholebodydynamics.xml 18 | @ONLY) 19 | 20 | yarp_install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${WBD_YARP_ROBOT_NAME}/wholebodydynamics.xml 21 | DESTINATION ${YARP_ROBOTS_INSTALL_DIR}/${WBD_YARP_ROBOT_NAME}/estimators) 22 | endif() 23 | 24 | # Configure and install files for using wholebodydynamics externally 25 | configure_file(${CMAKE_CURRENT_SOURCE_DIR}/${WBD_EXTERNAL_WBD_CONF} 26 | ${CMAKE_CURRENT_BINARY_DIR}/${WBD_YARP_ROBOT_NAME}/wholebodydynamics-external.xml 27 | @ONLY) 28 | 29 | yarp_install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${WBD_YARP_ROBOT_NAME}/wholebodydynamics-external.xml 30 | DESTINATION ${YARP_ROBOTS_INSTALL_DIR}/${WBD_YARP_ROBOT_NAME}/estimators) 31 | 32 | configure_file(${CMAKE_CURRENT_SOURCE_DIR}/${WBD_EXTERNAL_YRI_CONF} 33 | ${CMAKE_CURRENT_BINARY_DIR}/${WBD_YARP_ROBOT_NAME}/launch-wholebodydynamics.xml 34 | @ONLY) 35 | 36 | yarp_install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${WBD_YARP_ROBOT_NAME}/launch-wholebodydynamics.xml 37 | DESTINATION ${YARP_ROBOTS_INSTALL_DIR}/${WBD_YARP_ROBOT_NAME}) 38 | endmacro() 39 | 40 | 41 | # The wholebodydynamics device is a generic device, but at the moment is used mainly in iCub robots. 42 | # 43 | # For this reason, there is a need to have robot-specific configuration files to deal with the different configurations 44 | # in the robots (different number of FT sensors, etc, etc). However most of this files are shared across robots, 45 | # and so we just have a fixed number of configuration files that are installed for several robots. 46 | # 47 | # The main differences across iCub's with both arms and legs are: 48 | # * The number of the FT sensors, either 4 or 6. 49 | # As every F/T sensor is mapped to a single YARP device, it is necessary to change the attach list of the device. 50 | # * Depending on whether the wholebodydynamics is running in the main yarprobotinterface of an ETH icub, 51 | # or on the yarprobotinterface of a CAN robot, or running externally, the devices that export the IVirtualAnalogSensor 52 | # interface change: for ETH robots this interfaces are exposed by the same devices that expose the controlboard interfaces, 53 | # while for wbd running externally and CAN robots this interfaces are exposed by specific devices. 54 | # 55 | 56 | # A good list of robots are eth with 6 ft sensors, fortunatly 57 | set(ICUBS_ETH_SIX_FTS iCubDarmstadt01;iCubGenova02;iCubGenova04;iCubNancy01) 58 | foreach(YARP_ROBOT_NAME ${ICUBS_ETH_SIX_FTS}) 59 | message(STATUS ${YARP_ROBOT_NAME}) 60 | install_wbd_robots_files(YARP_ROBOT_NAME ${YARP_ROBOT_NAME} 61 | EXTERNAL_WBD_CONF wholebodydynamics-icub-external-and-can-six-fts.xml 62 | EXTERNAL_YRI_CONF launch-wholebodydynamics-icub-six-fts.xml 63 | ROBOT_WBD_CONF wholebodydynamics-icub-eth-six-fts.xml) 64 | endforeach() 65 | 66 | # We have also several robots that are CAN with 6 ft sensors 67 | # For them, there is no way to work because CanBusVirtualAnalogSensor does not support the IAxisInfo interface 68 | set(ICUBS_CAN_SIX_FTS iCubGenova01;iCubGenova03;iCubParis01;iCubParis02) 69 | foreach(YARP_ROBOT_NAME ${ICUBS_CAN_SIX_FTS}) 70 | message(STATUS ${YARP_ROBOT_NAME}) 71 | install_wbd_robots_files(YARP_ROBOT_NAME ${YARP_ROBOT_NAME} 72 | EXTERNAL_WBD_CONF wholebodydynamics-icub-external-and-can-six-fts.xml 73 | EXTERNAL_YRI_CONF launch-wholebodydynamics-icub-six-fts.xml 74 | NO_ROBOT) 75 | endforeach() 76 | 77 | # 78 | # In the Gazebo simulation no yarprobotinterface is explicitly instantiated, so we install only the remote version 79 | # 80 | install_wbd_robots_files(YARP_ROBOT_NAME icubGazeboSim 81 | EXTERNAL_WBD_CONF wholebodydynamics-icub-external-six-fts-sim.xml 82 | EXTERNAL_YRI_CONF launch-wholebodydynamics-icub-six-fts-sim.xml 83 | NO_ROBOT) 84 | 85 | install_wbd_robots_files(YARP_ROBOT_NAME iCubGazeboV2_5 86 | EXTERNAL_WBD_CONF wholebodydynamics-icub-external-six-fts-sim.xml 87 | EXTERNAL_YRI_CONF launch-wholebodydynamics-icub-six-fts-sim.xml 88 | NO_ROBOT) 89 | 90 | install_wbd_robots_files(YARP_ROBOT_NAME iCubGazeboV3 91 | EXTERNAL_WBD_CONF wholebodydynamics-icub3-external-sim.xml 92 | EXTERNAL_YRI_CONF launch-wholebodydynamics-icub3-sim.xml 93 | NO_ROBOT) 94 | 95 | 96 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # whole-body-estimators ![C++ CI Workflow](https://github.com/robotology/whole-body-estimators/workflows/C++%20CI%20Workflow/badge.svg) 2 | YARP-based estimators for humanoid robots. 3 | 4 | # Overview 5 | - [:orange_book: Implementation](#orange_book-implementation) 6 | - [:page_facing_up: Dependencies](#page_facing_up-dependencies) 7 | - [:hammer: Installation](#installation) 8 | - [Usage](#usage) 9 | - [Authors](#authors) 10 | 11 | # :orange_book: Implementation 12 | The current implementations available in the `devices` folder include, 13 | - `baseEstimatorV1` includes a simple humanoid floating base estimation algorithm fusing legged odometry and IMU attitude estimation, aimed to be used along side walking controllers. 14 | 15 | ![Floating Base Estimation Algorithm V1](doc/resources/fbeV1.png) 16 | 17 | - `wholeBodyDynamics` primarily implements external contact wrench and internal joint torques estimation using various sensor modalities of the robot. Please see the document [Force Control on iCub](doc/howto/force_control_on_icub.md) for more details (This is ported from its original archived repository [robotology-legacy/codyco-modules](https://github.com/robotology-legacy/codyco-modules)). It additionally includes supporting algorithms for gravity compensation torques estimation, and force-torque sensors calibration. 18 | 19 | 20 | # :page_facing_up: Dependencies 21 | * [YCM](https://github.com/robotology/ycm) extra CMake Modules for YARP and friends. 22 | * [YARP](http://www.yarp.it/): to handle the comunication with the robot; 23 | * [ICUB](https://github.com/robotology/icub-main): to use the utilities like low pass filters from the `ctrLib` library 24 | * [iDynTree](https://github.com/robotology/idyntree/tree/devel): to setup the floating base estimation algorithm. Please compile iDynTree with CMake option IDYNTREE_USES_ICUB_MAIN ON (depends on ICUB). 25 | * [Gazebo](http://gazebosim.org/): for the simulation (tested Gazebo 8 and 9). 26 | 27 | ## Optional Dependencies 28 | * [walking-controllers](https://github.com/robotology/walking-controllers): to test the floating base estimation along side walking controllers 29 | 30 | It must be noted that all of these dependencies can be directly installed together in one place using the [robotology-superbuild](https://github.com/robotology/robotology-superbuild), in particular enabling its `Dynamics` component. 31 | 32 | # :hammer: Installation 33 | 34 | ## conda (recommended) 35 | 36 | You can easily install `whole-body-estimators` with with `conda` using the following command 37 | ~~~ 38 | conda install -c conda-forge -c robotology whole-body-estimators 39 | ~~~ 40 | 41 | If you are not familiar with conda or conda-forge, you can read an introduction document in [conda-forge overview](https://github.com/robotology/robotology-superbuild/blob/master/doc/conda-forge.md#conda-forge-overview). 42 | 43 | ## robotology-superbuild (advanced) 44 | 45 | If you are installing `whole-body-estimators` for use as part of [iCub humanoid robot software installation](https://icub-tech-iit.github.io/documentation/sw_installation/), you may want to install `whole-body-estimators` through the [robotology-superbuild](https://github.com/robotology/robotology-superbuild), an easy way to download, compile and install the robotology software on multiple operating systems, using the [CMake](https://www.cmake.org) build system and its extension [YCM](http://robotology.github.io/ycm). To get `whole-body-estimators` when using the `robotology-superbuild`, please enable the `ROBOTOLOGY_ENABLE_DYNAMICS` CMake option of the superbuild. 46 | 47 | 48 | ## Build from source with pixi (advanced) 49 | 50 | If you are just interesting in building the devices to check that the compilation is working fine and tests pass, you can use pixi: 51 | 52 | ~~~ 53 | git clone https://github.com/robotology/whole-body-estimators.git 54 | cd whole-body-estimators 55 | pixi run download_deps 56 | pixi run install_deps 57 | pixi run test 58 | ~~~ 59 | 60 | ## Build from source manually (advanced) 61 | 62 | ### Linux 63 | 64 | ```sh 65 | git clone https://github.com/robotology/whole-body-estimators.git 66 | cd whole-body-estimators 67 | mkdir build && cd build 68 | cmake ../ 69 | make 70 | [sudo] make install 71 | ``` 72 | 73 | Notice: `sudo` is not necessary if you specify the `CMAKE_INSTALL_PREFIX`. In this case it is necessary to add in the `.bashrc` or `.bash_profile` the following lines: 74 | ``` sh 75 | export WBDEstimator_INSTALL_DIR=/path/where/you/installed 76 | export YARP_DATA_DIRS=${YARP_DATA_DIRS}:${WBDEstimator_INSTALL_DIR}/share/yarp 77 | ``` 78 | Note that this is not necessary if you install `whole-body-estimators` via the `robotology-superbuild` . 79 | 80 | # Usage 81 | 82 | - [`baseEstimatorV1`](devices/baseEstimatorV1/README.md) Please follow the documentation available here to run the floating base estimator. 83 | - [`wholeBodyDynamics`](devices/wholeBodyDynamics/README.md) Please follow the documentation for a description of features/parameters of the device and please follow the documentation in [Force Control on iCub](doc/howto/force_control_on_icub.md) for running the whole body dynamics device. 84 | 85 | # Authors 86 | 87 | ``` 88 | Hosameldin Awadalla Omer Mohamed 89 | Francisco Javier Andrade Chavez 90 | Prashanth Ramadoss 91 | Giulio Romualdi 92 | Silvio Traversaro 93 | Daniele Pucci 94 | ``` 95 | -------------------------------------------------------------------------------- /devices/virtualAnalogRemapper/VirtualAnalogRemapper.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016 Fondazione Istituto Italiano di Tecnologia 3 | * Author: Silvio Traversaro 4 | * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT 5 | */ 6 | 7 | #ifndef YARP_DEV_VIRTUALANALOGREMAPPER_H 8 | #define YARP_DEV_VIRTUALANALOGREMAPPER_H 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | namespace yarp { 19 | namespace dev { 20 | 21 | /** 22 | * Structure of information relative to a remapped axis. 23 | */ 24 | struct virtualAnalogSensorRemappedAxis 25 | { 26 | IVirtualAnalogSensor * dev; 27 | IAxisInfo * devInfo; 28 | int localAxis; 29 | int devIdx; 30 | bool useVectorUpdateMeasure; 31 | }; 32 | 33 | /** 34 | * Structure of information relative to a remapped subdevice. 35 | */ 36 | struct virtualAnalogSensorRemappedSubdevice 37 | { 38 | IVirtualAnalogSensor * dev; 39 | yarp::sig::Vector measureBuffer; 40 | std::vector local2globalIdx; 41 | bool useVectorUpdateMeasure; 42 | }; 43 | 44 | 45 | 46 | 47 | /** 48 | * @ingroup dev_impl_wrapper 49 | * 50 | * \brief Device that remaps multiple device exposing a IVirtualAnalogSensor to a have them behave as a single device. 51 | * 52 | * \section VirtualAnalogRemapper Description of input parameters 53 | * 54 | * This device will connect to multiple devices exposing the IVirtualAnalogSensor interface, 55 | * exposing them as a single device. 56 | * 57 | * The remapping is done using the IAxisInfo interface. 58 | * 59 | * The main difference with respect to the style of remapping provided by the ControlBoardRemapper 60 | * is that the IVirtualAnalogSensor does not provide updateMeasure methods for a subset of the axes. 61 | * Using just the single-axis version is tipically doable when this device is running on the robot, but 62 | * it may be prohibitly expensive to do when the measure are published on a port, as each call to the 63 | * single-axis updateMeasure may involve to send a packed. 64 | * Consequently if the VirtualAnalogRemapper detects that all channels in a subdevice are part 65 | * of the remapped device, the vector-value updateMeasure method will be used. 66 | * 67 | * 68 | * Parameters required by this device are: 69 | * | Parameter name | SubParameter | Type | Units | Default Value | Required | Description | Notes | 70 | * |:--------------:|:--------------:|:-------:|:--------------:|:-------------:|:--------------------------: |:-----------------------------------------------------------------:|:-----:| 71 | * | axesNames | - | vector of strings | - | - | Yes | Ordered list of the axes that are part of the remapped device. | | 72 | * 73 | * The axes are then mapped to the wrapped controlboard in the attachAll method, using the 74 | * values returned by the getAxisName method of the attached devices. 75 | * 76 | * Configuration file using .ini format. 77 | * 78 | * \code{.unparsed} 79 | * device virtualAnalogRemapper 80 | * axesNames (joint1 joint2 joint3) 81 | * ... 82 | * \endcode 83 | * 84 | **/ 85 | class VirtualAnalogRemapper: public DeviceDriver, 86 | public IVirtualAnalogSensor, 87 | public IMultipleWrapper, 88 | public IAxisInfo 89 | { 90 | protected: 91 | 92 | std::vector m_axesNames; 93 | 94 | /** 95 | * Vector containg the information about a specific axis remapped by this device. 96 | * This is configured during the attachAll method. 97 | * This vector will always be of size getChannels() 98 | */ 99 | std::vector remappedAxes; 100 | 101 | /** 102 | * Vector containing the information about a specific subdevice remapped by this device. 103 | * This vector will have the dimension of the number of mapper subdevices, 104 | * but will be used only if for a specific subdevice it would be detected that all the channels 105 | * are part of the remapper device. 106 | */ 107 | std::vector remappedSubdevices; 108 | 109 | /** 110 | * Get the number of remapped devices. 111 | */ 112 | int getNrOfSubDevices(); 113 | 114 | 115 | public: 116 | VirtualAnalogRemapper(); 117 | virtual ~VirtualAnalogRemapper(); 118 | 119 | /* DeviceDriver methods (documented in DeviceDriver class) */ 120 | virtual bool open(yarp::os::Searchable& config); 121 | virtual bool close(); 122 | 123 | /* IVirtualAnalogSensor methods (documented in IVirtualAnalogSensor class) */ 124 | virtual VAS_status getVirtualAnalogSensorStatus(int ch); 125 | virtual int getVirtualAnalogSensorChannels(); 126 | virtual bool updateVirtualAnalogSensorMeasure(yarp::sig::Vector &measure); 127 | virtual bool updateVirtualAnalogSensorMeasure(int ch, double &measure); 128 | 129 | /** IAxisInfo methods (documented in IVirtualAnalogSensor class) */ 130 | virtual bool getAxisName(int axis, std::string& name); 131 | virtual bool getJointType(int axis, yarp::dev::JointTypeEnum& type); 132 | virtual bool getAxes(int* ax); 133 | 134 | /** IMultipleWrapper methods (documented in IMultipleWrapper */ 135 | virtual bool attachAll(const PolyDriverList &p); 136 | virtual bool detachAll(); 137 | }; 138 | 139 | } 140 | } 141 | 142 | #endif // YARP_DEV_VIRTUALANALOGREMAPPER_H 143 | -------------------------------------------------------------------------------- /devices/baseEstimatorV1/scope/contact_scope.xml: -------------------------------------------------------------------------------- 1 | 8 | 9 | 10 | 19 | 20 | 26 | 27 | 33 | 34 | 35 | 36 | 45 | 46 | 52 | 53 | 59 | 60 | 61 | 62 | 71 | 72 | 78 | 79 | 85 | 86 | 87 | 88 | 89 | 90 | 99 | 100 | 106 | 107 | 113 | 114 | 115 | 116 | 125 | 126 | 132 | 133 | 139 | 140 | 141 | 142 | 151 | 152 | 158 | 159 | 165 | 166 | 167 | 168 | 169 | 170 | -------------------------------------------------------------------------------- /devices/baseEstimatorV1/README.md: -------------------------------------------------------------------------------- 1 | # Overview 2 | This estimator uses the kinematics, IMU mounted on the head and the contact forces information to estimate the floating base of the robot. The pose of floating base with respect to the inertial frame is computed through legged odometry and is fused along with the attitude estimates from the head IMU. The base velocity is obtained from the contact Jacobian computed using the kinematics information by enforcing the unilateral constraint of the foot. 3 | 4 | - [:computer: How to run the simulation](#computer-how-to-run-the-simulation) 5 | - [:running: How to test on iCub](#running-how-to-test-on-icub) 6 | 7 | # :computer: How to run the simulation 8 | 1. Set the `YARP_ROBOT_NAME` environment variable according to the chosen Gazebo model: 9 | ```sh 10 | export YARP_ROBOT_NAME="iCubGazeboV2_5" 11 | ``` 12 | 2. Run `yarpserver` 13 | ``` sh 14 | yarpserver --write 15 | ``` 16 | 3. Run gazebo and drag and drop iCub (e.g. icubGazeboSim or iCubGazeboV2_5): 17 | 18 | ``` sh 19 | gazebo -slibgazebo_yarp_clock.so 20 | ``` 21 | 4. Prior to launching the `baseEstimatorV1`, launch the `wholeBodyDynamics` device. 22 | 23 | ``` sh 24 | YARP_CLOCK=/clock yarprobotinterface --config launch-wholebodydynamics.xml 25 | ``` 26 | 5. Reset the offset of the FT sensors. Open a terminal and write 27 | 28 | ``` 29 | yarp rpc /wholeBodyDynamics/rpc 30 | >> resetOffset all 31 | ``` 32 | Note that this step can be avoided if a flag `startWithZeroFTSensorOffsets` is set to true in the `wholeBodyDynamics` configuration file (This is applicable only in the simulation.). 33 | 6. Run `yarprobotinterface` with corresponding device configuration file. 34 | - To run `baseEstimatorV1`, 35 | 36 | ``` sh 37 | YARP_CLOCK=/clock yarprobotinterface --config launch-fbe-analogsens.xml 38 | ``` 39 | This launches the floating base estimation device. 40 | 7. communicate with the `base-estimator` through RPC service calls: 41 | ``` 42 | yarp rpc /base-estimator/rpc 43 | ``` 44 | the following commands are allowed: 45 | * `startFloatingBaseFilter`: starts the estimator, this needs to be run after the FT sensors are reset; 46 | 47 | other optional commands include, 48 | 49 | * `setContactSchmittThreshold lbreak lmake rbreak rmake`: used to set contact force thresholds for feet contact detection; 50 | * `setPrimaryFoot foot`: set the foot to the one that does not break the contact initially during walking, `foot` can be `left` or `right`; 51 | * `useJointVelocityLPF flag`: use a low pass filter on the joint velocities, `flag` can be `true` or `false`; 52 | * `setJointVelocityLPFCutoffFrequency freq`: set the cut-off frequency for the low pass filter on the joint velocities; 53 | * `resetLeggedOdometry`: reset the floating base pose and reset the legged odometry to the inital state; 54 | * `resetLeggedOdometryWithRefFrame frame x y z roll pitch yaw`: reset the legged odometry by mentioning an initial reference to the world frame with respect to the initial fixed frame; 55 | * `getRefFrameForWorld`: get the initial fixed frame with respect to which the world frame was set; 56 | 57 | ## Configuration 58 | 59 | The configuration file for the estimator can be found `app/robots/${YARP_ROBOT_NAME}/fbe-analogsens.xml`. 60 | The attitude estimation for the IMU can be chosen to be either a QuaternionEKF or a non-linear complementary filter. The gains should be tuned accordingly. The choice of which IMU to use can also be chosen, however please read the note below. 61 | 62 | ## How to dump data 63 | Before run `yarprobotinterface` check if [`dump_data`](app/robots/iCubGazeboV2_5/fbe-analogsens.xml#L14) is set to `true` 64 | 65 | If `true`, run the Logger Module 66 | ``` sh 67 | YARP_CLOCK=/clock WalkingLoggerModule 68 | ``` 69 | 70 | All the data will be saved in the current folder inside a `txt` named `Dataset_YYYY_MM_DD_HH_MM_SS.txt` 71 | 72 | # :running: How to test on iCub 73 | You can follow the same instructions of the simulation section without using `YARP_CLOCK=/clock`. Make sure your `YARP_ROBOT_NAME` is coherent with the name of the robot (e.g. iCubGenova04) 74 | ## :warning: Warning 75 | Currently the supported robots are only: 76 | - ``iCubGenova04`` 77 | - ``icubGazeboV2_5`` 78 | 79 | --- 80 | **Note** 81 | If you would like to use root link IMU instead of head IMU (default is Head IMU), please uncomment the following lines in `launch-fbe-analogsens.xml` 82 | ``` xml 83 | 84 | /icubSim/xsens_inertial 85 | /baseestimation/waist/imu:i 86 | 87 | ``` 88 | 89 | and the following line in `fbe-analogsens.xml` 90 | ```xml 91 | xsens_inertial 92 | ``` 93 | --- 94 | # Troubleshooting 95 | ## yarprobotinterface closes with the message, "baseEstimatorV1 device not found" 96 | In order to check if the `baseEstimatorV1` device has been properly installed and `YARP` is being able to identify the location of the device library, we can run 97 | ```sh 98 | yarpdev --list 99 | ``` 100 | to check if `baseEstimatorV1` plugin has been listed with the set of YARP devices with the following message 101 | ```sh 102 | [INFO]Device "baseEstimatorV1", available on request 103 | ``` 104 | 105 | In case this is not seen, check again properly if the `YARP_DATA_DIRS` has been properly updated. 106 | 107 | If the following error message is seen, 108 | ```sh 109 | [Warning]Device "baseEstimatorV1", Wrong library name 110 | ``` 111 | then, it can be fixed by adding the following line in your `.bashrc` file 112 | ```sh 113 | export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:${WBDEstimator_INSTALL_DIR}/lib/yarp 114 | ``` 115 | 116 | and run `sudo ldconfig` on your terminal. 117 | -------------------------------------------------------------------------------- /devices/baseEstimatorV1/app/robots/iCubGenova04/estimators/fbe-analogsens.xml: -------------------------------------------------------------------------------- 1 | 8 | 9 | 10 | 11 | model.urdf 12 | icub 13 | 0.010 14 | ("neck_pitch", "neck_roll", "neck_yaw", "torso_pitch", "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll", "l_arm_ft_sensor", "r_arm_ft_sensor", "l_leg_ft_sensor", "r_leg_ft_sensor", "l_foot_ft_sensor", "r_foot_ft_sensor") 15 | root_link 16 | l_foot_ft 17 | r_foot_ft 18 | head_imu_acc_1x1 19 | head 20 | ("neck_pitch", "neck_roll", "neck_yaw", "torso_pitch", "torso_roll", "torso_yaw") 21 | (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) 22 | 23 | false 24 | false 25 | true 26 | 3.0 27 | 28 | 29 | 30 | l_sole 31 | l_sole 32 | (0 0 0 0 0 0) 33 | 34 | 35 | qekf 36 | (1.0 0.0 0.0 0.0) 37 | 0.0 38 | 0.0 39 | 40 | 41 | 0.010 42 | 0.7 43 | 0.001 44 | false 45 | (0 0 0) 46 | 47 | 48 | 0.010 49 | 0.03 50 | 0.0 51 | 0.5 52 | 10e-11 53 | 10e-6 54 | 10e-1 55 | 10e-11 56 | 10e-3 57 | false 58 | 59 | 60 | right 61 | 0.5 62 | 0.5 63 | 130.0 64 | 25.0 65 | 150.0 66 | 25.0 67 | 68 | 69 | /wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o 70 | /wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o 71 | 72 | 73 | /logger/data:o 74 | /logger/rpc:o 75 | /logger-est/data:i 76 | /logger-est/rpc:i 77 | 78 | 79 | 80 | 81 | all_joints_mc 82 | 83 | inertial 84 | 85 | 86 | left_upper_arm_strain 87 | right_upper_arm_strain 88 | left_upper_leg_strain 89 | right_upper_leg_strain 90 | left_lower_leg_strain 91 | right_lower_leg_strain 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | -------------------------------------------------------------------------------- /devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/estimators/fbe-analogsens.xml: -------------------------------------------------------------------------------- 1 | 8 | 9 | 10 | 11 | 12 | model.urdf 13 | icubSim 14 | 0.010 15 | ("neck_pitch", "neck_roll", "neck_yaw", "torso_pitch", "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll", "l_arm_ft_sensor", "r_arm_ft_sensor", "l_leg_ft_sensor", "r_leg_ft_sensor", "l_foot_ft_sensor", "r_foot_ft_sensor") 16 | root_link 17 | l_foot_ft 18 | r_foot_ft 19 | head_imu_acc_1x1 20 | head 21 | ("neck_pitch", "neck_roll", "neck_yaw", "torso_pitch", "torso_roll", "torso_yaw") 22 | (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) 23 | 24 | false 25 | false 26 | false 27 | 10.0 28 | 29 | 30 | l_sole 31 | l_sole 32 | (0 0 0 0 0 0) 33 | 34 | 35 | qekf 36 | (1.0 0.0 0.0 0.0) 37 | 0.0 38 | 0.0 39 | 40 | 41 | 0.010 42 | 0.7 43 | 0.001 44 | false 45 | (0 0 0) 46 | 47 | 48 | 0.010 49 | 0.03 50 | 0.0 51 | 0.5 52 | 10e-11 53 | 10e-6 54 | 10e-1 55 | 10e-11 56 | 10e-3 57 | false 58 | 59 | 60 | right 61 | 0.01 62 | 0.01 63 | 130.0 64 | 25.0 65 | 130.0 66 | 25.0 67 | 68 | 69 | /wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o 70 | /wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o 71 | 72 | 73 | /logger/data:o 74 | /logger/rpc:o 75 | /logger/data:i 76 | /logger/rpc:i 77 | 78 | 79 | 80 | 81 | all_joints_mc 82 | 83 | inertial 84 | 85 | 86 | left_upper_arm_strain 87 | right_upper_arm_strain 88 | left_upper_leg_strain 89 | right_upper_leg_strain 90 | left_lower_leg_strain 91 | right_lower_leg_strain 92 | 93 | 94 | 95 | 96 | 97 | 98 | -------------------------------------------------------------------------------- /devices/baseEstimatorV1/scope/base_velocity.xml: -------------------------------------------------------------------------------- 1 | 8 | 9 | 10 | 19 | 20 | 26 | 27 | 33 | 34 | 40 | 41 | 42 | 43 | 52 | 53 | 59 | 60 | 66 | 72 | 73 | 74 | 75 | 84 | 85 | 91 | 92 | 98 | 99 | 100 | 106 | 107 | 108 | 109 | 110 | 111 | 120 | 121 | 127 | 128 | 134 | 135 | 136 | 142 | 143 | 144 | 145 | 154 | 155 | 161 | 162 | 168 | 169 | 170 | 176 | 177 | 178 | 179 | 188 | 189 | 195 | 196 | 202 | 203 | 209 | 210 | 211 | 212 | 213 | 214 | -------------------------------------------------------------------------------- /devices/wholeBodyDynamics/test/fakeFTs/fakeFTs.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia 2 | // SPD X-License-Identifier: BSD-3-Clause 3 | 4 | 5 | #include "fakeFTs.h" 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | using namespace yarp::os; 17 | using namespace yarp::dev; 18 | using namespace yarp::sig; 19 | using namespace yarp::math; 20 | 21 | namespace { 22 | YARP_LOG_COMPONENT(fakeFTs, "yarp.device.fakeFTs") 23 | constexpr double DEFAULT_PERIOD = 0.01; // seconds 24 | constexpr double DEFAULT_DUMMY_VALUE = 0.0; 25 | } 26 | 27 | /** 28 | * This device implements a fake analog sensor 29 | * emulating an IMU 30 | */ 31 | fakeFTs::fakeFTs() : 32 | PeriodicThread(DEFAULT_PERIOD) 33 | { 34 | } 35 | 36 | fakeFTs::~fakeFTs() 37 | { 38 | close(); 39 | } 40 | 41 | bool fakeFTs_getConfigParamsAsList(yarp::os::Searchable& config,std::string propertyName , std::vector & list, bool isRequired) 42 | { 43 | yarp::os::Property prop; 44 | prop.fromString(config.toString().c_str()); 45 | yarp::os::Bottle *propNames=prop.find(propertyName).asList(); 46 | if(propNames==nullptr) 47 | { 48 | if(isRequired) 49 | { 50 | yError() <<"fakeFTs: Error parsing parameters: \" "<size()); 56 | for(auto elem=0u; elem < propNames->size(); elem++) 57 | { 58 | list[elem] = propNames->get(elem).asString().c_str(); 59 | } 60 | 61 | return true; 62 | } 63 | 64 | bool fakeFTs::open(yarp::os::Searchable &config) 65 | { 66 | double period; 67 | if( config.check("period")) { 68 | period = config.find("period").asInt32() / 1000.0; 69 | setPeriod(period); 70 | } else { 71 | yInfo() << "Using default period of " << DEFAULT_PERIOD << " s"; 72 | } 73 | 74 | bool required = true; 75 | bool ok = fakeFTs_getConfigParamsAsList(config, "sensorNames", m_sensorNames, required); 76 | 77 | if (!ok) { 78 | return false; 79 | } 80 | 81 | start(); 82 | return true; 83 | } 84 | 85 | bool fakeFTs::close() 86 | { 87 | fakeFTs::stop(); 88 | return true; 89 | } 90 | 91 | bool fakeFTs::threadInit() 92 | { 93 | lastStamp.update(); 94 | return true; 95 | } 96 | 97 | void fakeFTs::run() 98 | { 99 | static double count=10; 100 | 101 | lastStamp.update(); 102 | 103 | dummy_value = count; 104 | if (!constantValue) { 105 | count++; 106 | } 107 | 108 | if (count >= 360) { 109 | count = 0; 110 | } 111 | } 112 | 113 | size_t fakeFTs::genericGetNrOfSensors() const 114 | { 115 | return m_sensorNames.size(); 116 | } 117 | 118 | yarp::dev::MAS_status fakeFTs::genericGetStatus(size_t sens_index) const 119 | { 120 | if (sens_index>m_sensorNames.size()) { 121 | return yarp::dev::MAS_status::MAS_ERROR; 122 | } 123 | 124 | return yarp::dev::MAS_status::MAS_OK; 125 | } 126 | 127 | bool fakeFTs::genericGetSensorName(size_t sens_index, std::string &name) const 128 | { 129 | if (sens_index>m_sensorNames.size()) { 130 | return false; 131 | } 132 | 133 | name = m_sensorNames[sens_index]; 134 | return true; 135 | } 136 | 137 | bool fakeFTs::genericGetFrameName(size_t sens_index, std::string &frameName) const 138 | { 139 | if (sens_index>m_sensorNames.size()) { 140 | return false; 141 | } 142 | 143 | frameName = m_sensorNames[sens_index]; 144 | return true; 145 | } 146 | 147 | // ---------------------- ITemperatureSensors -------------------------------------------------------- 148 | size_t fakeFTs::getNrOfTemperatureSensors() const 149 | { 150 | return genericGetNrOfSensors(); 151 | } 152 | 153 | yarp::dev::MAS_status fakeFTs::getTemperatureSensorStatus(size_t sens_index) const 154 | { 155 | return genericGetStatus(sens_index); 156 | } 157 | 158 | bool fakeFTs::getTemperatureSensorName(size_t sens_index, std::string &name) const 159 | { 160 | return genericGetSensorName(sens_index, name); 161 | } 162 | 163 | bool fakeFTs::getTemperatureSensorFrameName(size_t sens_index, std::string &frameName) const 164 | { 165 | return genericGetFrameName(sens_index, frameName); 166 | } 167 | 168 | bool fakeFTs::getTemperatureSensorMeasure(size_t sens_index, double& out, double& timestamp) const 169 | { 170 | if (sens_index>m_sensorNames.size()) { 171 | return false; 172 | } 173 | 174 | out = dummy_value; 175 | timestamp = lastStamp.getTime(); 176 | 177 | return true; 178 | } 179 | 180 | bool fakeFTs::getTemperatureSensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const 181 | { 182 | out.resize(1); 183 | out[0] = dummy_value; 184 | timestamp = lastStamp.getTime(); 185 | return true; 186 | } 187 | 188 | //------------------------- ISixAxisForceTorqueSensors ------------------------- 189 | 190 | size_t fakeFTs::getNrOfSixAxisForceTorqueSensors() const 191 | { 192 | return genericGetNrOfSensors(); 193 | } 194 | 195 | yarp::dev::MAS_status fakeFTs::getSixAxisForceTorqueSensorStatus(size_t sens_index) const 196 | { 197 | return genericGetStatus(sens_index); 198 | } 199 | 200 | bool fakeFTs::getSixAxisForceTorqueSensorName(size_t sens_index, std::string &name) const 201 | { 202 | return genericGetSensorName(sens_index, name); 203 | } 204 | 205 | bool fakeFTs::getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string &frameName) const 206 | { 207 | return genericGetFrameName(sens_index, frameName); 208 | } 209 | 210 | bool fakeFTs::getSixAxisForceTorqueSensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const 211 | { 212 | if (sens_index>m_sensorNames.size()) { 213 | return false; 214 | } 215 | 216 | out.resize(6); 217 | for (size_t i=0; i < 6; i++) { 218 | out[i] = dummy_value; 219 | } 220 | timestamp = lastStamp.getTime(); 221 | 222 | return true; 223 | } 224 | -------------------------------------------------------------------------------- /devices/wholeBodyDynamics/app/launch-wholebodydynamics-icub-four-fts.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | /icub/torso 8 | /wholeBodyDynamics/torso 9 | 10 | 11 | 12 | /icub/left_arm 13 | /wholeBodyDynamics/left_arm 14 | 15 | 16 | 17 | /icub/right_arm 18 | /wholeBodyDynamics/right_arm 19 | 20 | 21 | 22 | /icub/left_leg 23 | /wholeBodyDynamics/left_leg 24 | 25 | 26 | 27 | /icub/right_leg 28 | /wholeBodyDynamics/right_leg 29 | 30 | 31 | 32 | /icub/head 33 | /wholeBodyDynamics/head 34 | 35 | 36 | 37 | 38 | /icub/joint_vsens/torso:i 39 | /wholeBodyDynamics/torso/Torques:o 40 | (torso_yaw,torso_roll,torso_pitch) 41 | 4 42 | 43 | 44 | 45 | /icub/joint_vsens/left_arm:i 46 | /wholeBodyDynamics/torso/left_arm:o 47 | (l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_prosup) 48 | 1 49 | 50 | 51 | 52 | /icub/joint_vsens/right_arm:i 53 | /wholeBodyDynamics/right_arm/Torques:o 54 | (r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_prosup) 55 | 4 56 | 57 | 58 | 59 | /icub/joint_vsens/left_leg:i 60 | /wholeBodyDynamics/left_leg/Torques:o 61 | (l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll) 62 | 2 63 | 64 | 65 | 66 | /icub/joint_vsens/right_leg:i 67 | /wholeBodyDynamics/right_leg/Torques:o 68 | (r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll) 69 | 2 70 | 71 | 72 | 94 | 95 | 96 | 97 | /icub/head/inertials 98 | /wholeBodyDynamics/imu 99 | 100 | 101 | 102 | 103 | /icub/left_arm/FT 104 | /wholeBodyDynamics/l_arm_ft 105 | 106 | 107 | 108 | /icub/right_arm/FT 109 | /wholeBodyDynamics/r_arm_ft 110 | 111 | 112 | 113 | /icub/left_leg/FT 114 | /wholeBodyDynamics/l_leg_ft 115 | 116 | 117 | 118 | /icub/right_leg/FT 119 | /wholeBodyDynamics/r_leg_ft 120 | 121 | 122 | 123 | 124 | 125 | -------------------------------------------------------------------------------- /devices/wholeBodyDynamics/app/launch-wholebodydynamics-icub-six-fts.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | /icub/torso 8 | /wholeBodyDynamics/torso 9 | 10 | 11 | 12 | /icub/left_arm 13 | /wholeBodyDynamics/left_arm 14 | 15 | 16 | 17 | /icub/right_arm 18 | /wholeBodyDynamics/right_arm 19 | 20 | 21 | 22 | /icub/left_leg 23 | /wholeBodyDynamics/left_leg 24 | 25 | 26 | 27 | /icub/right_leg 28 | /wholeBodyDynamics/right_leg 29 | 30 | 31 | 32 | /icub/head 33 | /wholeBodyDynamics/head 34 | 35 | 36 | 37 | 38 | /icub/joint_vsens/torso:i 39 | /wholeBodyDynamics/torso/Torques:o 40 | (torso_yaw,torso_roll,torso_pitch) 41 | 4 42 | 43 | 44 | 45 | /icub/joint_vsens/left_arm:i 46 | /wholeBodyDynamics/torso/left_arm:o 47 | (l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_prosup) 48 | 1 49 | 50 | 51 | 52 | /icub/joint_vsens/right_arm:i 53 | /wholeBodyDynamics/right_arm/Torques:o 54 | (r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_prosup) 55 | 4 56 | 57 | 58 | 59 | /icub/joint_vsens/left_leg:i 60 | /wholeBodyDynamics/left_leg/Torques:o 61 | (l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll) 62 | 2 63 | 64 | 65 | 66 | /icub/joint_vsens/right_leg:i 67 | /wholeBodyDynamics/right_leg/Torques:o 68 | (r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll) 69 | 2 70 | 71 | 72 | 94 | 95 | 96 | 97 | /icub/head/inertials 98 | /wholeBodyDynamics/imu 99 | 100 | 101 | 102 | 103 | /icub/left_arm/FT 104 | /wholeBodyDynamics/l_arm_ft 105 | 106 | 107 | 108 | /icub/right_arm/FT 109 | /wholeBodyDynamics/r_arm_ft 110 | 111 | 112 | 113 | /icub/left_leg/FT 114 | /wholeBodyDynamics/l_leg_ft 115 | 116 | 117 | 118 | /icub/right_leg/FT 119 | /wholeBodyDynamics/r_leg_ft 120 | 121 | 122 | 123 | /icub/left_foot/FT 124 | /wholeBodyDynamics/l_foot_ft 125 | 126 | 127 | 128 | /icub/right_foot/FT 129 | /wholeBodyDynamics/r_foot_ft 130 | 131 | 132 | 133 | 134 | 135 | -------------------------------------------------------------------------------- /devices/baseEstimatorV1/scope/base_scope.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 12 | 13 | 19 | 20 | 26 | 27 | 33 | 34 | 35 | 36 | 45 | 46 | 52 | 53 | 59 | 65 | 66 | 67 | 68 | 77 | 78 | 84 | 85 | 91 | 92 | 93 | 99 | 100 | 101 | 102 | 103 | 104 | 113 | 114 | 120 | 121 | 127 | 128 | 129 | 135 | 136 | 137 | 138 | 147 | 148 | 154 | 155 | 161 | 162 | 163 | 169 | 170 | 171 | 172 | 181 | 182 | 188 | 189 | 195 | 196 | 202 | 203 | 204 | 205 | 206 | -------------------------------------------------------------------------------- /libraries/ctrlLibRT/include/ctrlLibRT/filters.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 RobotCub Consortium, European Commission FP6 Project IST-004370 3 | * Author: Silvio Traversaro 4 | * email: silvio.traversaro@iit.it 5 | * website: www.robotcub.org 6 | * Permission is granted to copy, distribute, and/or modify this program 7 | * under the terms of the GNU General Public License, version 2 or any 8 | * later version published by the Free Software Foundation. 9 | * 10 | * A copy of the license can be found at 11 | * http://www.robotcub.org/icub/license/gpl.txt 12 | * 13 | * This program is distributed in the hope that it will be useful, but 14 | * WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General 16 | * Public License for more details 17 | */ 18 | 19 | /** 20 | * \defgroup Filters Filters 21 | * 22 | * @ingroup ctrlLibRT 23 | * 24 | * Classes for filtering, modified to avoid non-realtime behaviour. 25 | * 26 | * \author Ugo Pattacini, Silvio Traversaro. 27 | * 28 | */ 29 | 30 | #ifndef RT_FILTERS_H 31 | #define RT_FILTERS_H 32 | 33 | #include 34 | 35 | #include 36 | 37 | 38 | namespace iCub 39 | { 40 | 41 | namespace ctrl 42 | { 43 | 44 | namespace realTime 45 | { 46 | 47 | /** 48 | * \ingroup Filters 49 | * 50 | * IIR and FIR. 51 | */ 52 | class Filter 53 | { 54 | protected: 55 | Eigen::VectorXd b; 56 | Eigen::VectorXd a; 57 | yarp::sig::Vector y; 58 | 59 | Eigen::MatrixXd uold; ///< Matrix of past inputs: each column is a past sample 60 | int uold_last_column_sample; 61 | 62 | Eigen::MatrixXd yold; ///< Matrix of past outpts: each columns is a past output 63 | int yold_last_column_sample; 64 | 65 | size_t n; 66 | size_t m; 67 | 68 | public: 69 | /** 70 | * Creates a filter with specified numerator and denominator 71 | * coefficients. 72 | * @param num vector of numerator elements given as increasing 73 | * power of z^-1. 74 | * @param den vector of denominator elements given as increasing 75 | * power of z^-1. 76 | * @param y0 initial output. 77 | * @note den[0] shall not be 0. 78 | */ 79 | Filter(const yarp::sig::Vector &num, const yarp::sig::Vector &den, 80 | const yarp::sig::Vector &y0); 81 | 82 | /** 83 | * Internal state reset. 84 | * @param y0 new internal state. 85 | */ 86 | void init(const yarp::sig::Vector &y0); 87 | 88 | /** 89 | * Internal state reset for filter with zero gain. 90 | * @param y0 new internal state. 91 | * @param u0 expected next input. 92 | * @note The gain of a digital filter is the sum of the coefficients of its 93 | * numerator divided by the sum of the coefficients of its denumerator. 94 | */ 95 | void init(const yarp::sig::Vector &y0, const yarp::sig::Vector &u0); 96 | 97 | /** 98 | * Returns the current filter coefficients. 99 | * @param num vector of numerator elements returned as increasing 100 | * power of z^-1. 101 | * @param den vector of denominator elements returned as 102 | * increasing power of z^-1. 103 | */ 104 | void getCoeffs(yarp::sig::Vector &num, yarp::sig::Vector &den); 105 | 106 | /** 107 | * Sets new filter coefficients. 108 | * @param num vector of numerator elements given as increasing 109 | * power of z^-1. 110 | * @param den vector of denominator elements given as increasing 111 | * power of z^-1. 112 | * @note den[0] shall not be 0. 113 | * @note the internal state is reinitialized to the current 114 | * output. 115 | */ 116 | void setCoeffs(const yarp::sig::Vector &num, const yarp::sig::Vector &den); 117 | 118 | /** 119 | * Modifies the values of existing filter coefficients without 120 | * varying their lengths. 121 | * @param num vector of numerator elements given as increasing 122 | * power of z^-1. 123 | * @param den vector of denominator elements given as increasing 124 | * power of z^-1. 125 | * @return true/false on success/fail. 126 | * @note den[0] shall not be 0. 127 | * @note the adjustment is carried out iff num.size() and 128 | * den.size() match the existing numerator and denominator 129 | * lengths. 130 | */ 131 | bool adjustCoeffs(const yarp::sig::Vector &num, const yarp::sig::Vector &den); 132 | 133 | /** 134 | * Performs filtering on the actual input. 135 | * @param u reference to the actual input. 136 | * @return a reference the corresponding output. 137 | * @note the returned reference is valid till any new call to filt. 138 | */ 139 | const yarp::sig::Vector & filt(const yarp::sig::Vector &u); 140 | 141 | /** 142 | * Return the reference to the current filter output. 143 | * @return reference to the filter output. 144 | */ 145 | const yarp::sig::Vector & output() const { return y; } 146 | }; 147 | 148 | 149 | 150 | /** 151 | * \ingroup Filters 152 | * 153 | * First order low pass filter implementing the transfer function 154 | * H(s) = \frac{1}{1+\tau s} 155 | * 156 | */ 157 | class FirstOrderLowPassFilter 158 | { 159 | protected: 160 | Filter *filter; // low pass filter 161 | double fc; // cut frequency 162 | double Ts; // sample time 163 | yarp::sig::Vector y; // filter current output 164 | 165 | void computeCoeff(); 166 | 167 | public: 168 | /** 169 | * Creates a filter with specified parameters 170 | * @param cutFrequency cut frequency (Hz). 171 | * @param sampleTime sample time (s). 172 | * @param y0 initial output. 173 | */ 174 | FirstOrderLowPassFilter(const double cutFrequency, const double sampleTime, 175 | const yarp::sig::Vector &y0=yarp::sig::Vector(1,0.0)); 176 | 177 | /** 178 | * Destructor. 179 | */ 180 | ~FirstOrderLowPassFilter(); 181 | 182 | /** 183 | * Internal state reset. 184 | * @param y0 new internal state. 185 | */ 186 | void init(const yarp::sig::Vector &y0); 187 | 188 | /** 189 | * Change the cut frequency of the filter. 190 | * @param cutFrequency the new cut frequency (Hz). 191 | */ 192 | bool setCutFrequency(const double cutFrequency); 193 | 194 | /** 195 | * Change the sample time of the filter. 196 | * @param sampleTime the new sample time (s). 197 | */ 198 | bool setSampleTime(const double sampleTime); 199 | 200 | /** 201 | * Retrieve the cut frequency of the filter. 202 | * @return the cut frequency (Hz). 203 | */ 204 | double getCutFrequency() { return fc; } 205 | 206 | /** 207 | * Retrieve the sample time of the filter. 208 | * @return the sample time (s). 209 | */ 210 | double getSampleTime() { return Ts; } 211 | 212 | /** 213 | * Performs filtering on the actual input. 214 | * @param u reference to the actual input. 215 | * @return the corresponding output. 216 | */ 217 | const yarp::sig::Vector& filt(const yarp::sig::Vector &u); 218 | 219 | /** 220 | * Return current filter output. 221 | * @return the filter output. 222 | */ 223 | const yarp::sig::Vector& output() const { return y; } 224 | }; 225 | 226 | 227 | } 228 | 229 | } 230 | 231 | } 232 | 233 | #endif 234 | 235 | -------------------------------------------------------------------------------- /doc/howto/force_control_on_icub.md: -------------------------------------------------------------------------------- 1 | # Force Control on iCub 2 | 3 | **NOTE: This document was originally authored by Daniele Pucci, Silvio Traversaro and Francesco Romano. This document is directly ported from [robotology-legacy/codyco-modules/doc/force_control_on_icub.md](https://github.com/robotology-legacy/codyco-modules/blob/master/doc/force_control_on_icub.md).** 4 | 5 | This is a new and incomplete version of http://wiki.icub.org/wiki/Force_Control , updated with 6 | the details on the new `wholeBodyDynamicsDevice` module, which is in charge of estimating mainly joint torques and external forces, developed for the CoDyCo European Project. 7 | 8 | # History of joint-level torque control on iCub 9 | The first versions of iCub were not designed to perform joint-level torque control. 10 | To enable joint-level torque control, the iCub v1.1 was modified to include 4 six-axis 11 | force-torque sensors, embedded inside the arms and the legs of robot (2008-2010). [1] 12 | 13 | The original plan was to develop the iCub v2 with real strain-gauge based joint torque sensors, 14 | but this process [2] was not successfull and all existing iCub's uses the embedded six-axis force-torque 15 | sensors to estimate the joint torques for low-level joint torque control. 16 | 17 | During the CoDyCo project (2013-2017), the software originally developed in [1] was rewritten to improve 18 | its computational efficency and its generality (i.e. the possibility to run it on several different robots). 19 | 20 | # `wholebodydynamics` YARP device 21 | The `wholebodydynamics` YARP device (contained in the C++ class [`yarp::dev::WholeBodyDynamicsDevice`](http://wiki.icub.org/codyco/dox/html/classyarp_1_1dev_1_1WholeBodyDynamicsDevice.html)) 22 | is reading measurements of the embedded force-torque sensors, of the joint position and low-level estimates of joint velocity and accelerations and of 23 | one IMU mounted in the robot, and from this reading is estimating the external force-torques and internal joint torques of the robot. 24 | 25 | ## Differences with respect to the wholeBodyDynamics YARP module 26 | From the user perpective, the main differences w.r.t. to the wholeBodyDynamics YARP module are: 27 | * The estimation is performed using the iDynTree library, replacing the use of the iDyn library. 28 | * The model of the robot and of the sensor is loaded from a URDF model, as documented in https://github.com/robotology/idyntree/blob/master/doc/model_loading.md . 29 | This permits to run the estimation algorithm on arbitrary robot without modifyng the code, as in the case of the iCubHeidelberg01 that w.r.t. to normal iCub is missing the head and the arms. 30 | * The RPC interface is implemented using [YARP Thrift](http://www.yarp.it/thrift_tutorial_simple.html). 31 | This means that the `0` shorthand for performing the calibration of the force-torque offset is not supported anymore. The `calib` command however is compatible between the wholeBodyDynamics 32 | YARP module and the `wholebodydynamics` YARP device, so please use that one in your code to be compatible with both interfaces. 33 | * The functionality of the gravityCompensator module are now integrated in the `wholebodydynamics` device, and can be enabled/disabled using the parameters in the `GRAVITY_COMPENSATION` group. 34 | Furthermore, the gravity compensation torque offset is not sent anymore to the board if the axis control mode is set to `VOCAB_CM_TORQUE` . 35 | 36 | 37 | ## Run the `wholebodydynamics` YARP device 38 | Being a YARP device, it can run on the robot main PC in the robot's yarprobotinterface, or on an external pc using a separate yarprobotinterface. 39 | The `wholebodydynamics` device requires robot-specific configuration files, and currently this configuration files are provided for the following robots: 40 | 41 | | `YARP_ROBOT_NAME` | Number of F/T sensors | Internal electronics architecture | Support for running wholebodydynamics on the robot's yarprobotinterface | 42 | |:---:|:---:|:---:|:---:| 43 | | `icubGazeboSim` | 6 | N/A | NO | 44 | | `iCubDarmstadt01` | 6 | ETH | YES | 45 | | `iCubGenova02` | 6 | ETH | YES | 46 | | `iCubGenova04` | 6 | ETH | YES | 47 | | `iCubNancy01` | 6 | ETH | YES | 48 | | `iCubHeidelberg01` | 4 | ETH | YES | 49 | | `iCubGenova01` | 6 | CAN | NO | 50 | | `iCubGenova03` | 6 | CAN | NO | 51 | | `iCubParis01` | 6 | CAN | NO | 52 | | `iCubParis02` | 6 | CAN | NO | 53 | 54 | Note however that over time the configuration of this robots can change, and the configuration files contained in this repository may not be updated. Please check the status of the configuration files with the maintainer of this repository before using this configuration files. 55 | 56 | ### Run `wholebodydynamics` on an external PC 57 | This is the recommended procedure in general. To launch the `wholebodydynamics` on an external PC running a *nix based OS, just run: 58 | ~~~ 59 | YARP_ROBOT_NAME= yarprobotinterface --config launch-wholebodydynamics.xml 60 | ~~~ 61 | where `` is the robot for which you are launching the estimator. 62 | 63 | For example, if you want to run the `wholebodydynamics` for the Gazebo simulation, you will need to run: 64 | ~~~ 65 | YARP_ROBOT_NAME=icubGazeboSim yarprobotinterface --config launch-wholebodydynamics.xml 66 | ~~~ 67 | 68 | Note that you can avoid to preprend the `YARP_ROBOT_NAME=icubGazeboSim` environmental variable. 69 | 70 | ### Run `wholebodydynamics` on the robot 71 | **Please note that this configuration is not officially supported by the iCub Facility support team, so please request support only from this repo.** 72 | 73 | This setup requires adding one configuration file to the robot `yarprobotinterface` configuration files as distributed in the [robots-configuration](https://github.com/robotology/robots-configuration) repository. For doing this, please import the configuration files for your robot on the `~/.local/share/yarp/robots/${YARP_ROBOT_NAME}` using the `yarp-config` utility program. 74 | 75 | Assuming that `` is the build or installation directory of this repo, copy the file `/share/codyco/robots/${YARP_ROBOT_NAME}/estimators/wholebodydynamics.xml` 76 | in `~/.local/share/yarp/robots/${YARP_ROBOT_NAME}/estimators/wholebodydynamics.xml` (create the `estimators` directory if necessary). 77 | Then create a copy of the `icub_all.xml` file (or of any file that you want to use) called `icub_wbd.xml`. 78 | In `icub_wbd.xml`, add the following two lines before the `` closing tag: 79 | ~~~ 80 | 81 | 82 | ~~~ 83 | Now we can launch the `yarprobotinterface` with the `wholebodydynamics` device using the following command on the robot's PC: 84 | ~~~ 85 | yarprobotinterface --config icub_wbd.xml 86 | ~~~ 87 | 88 | It may be convenient to create a modified version of the `iCubStartup.xml` `yarpmanager` application on the terminal PC on which you launch the `yarpmanager`, 89 | called `iCubStartupWithWBD.xml`, in which the `--config icub_wbd.xml` parameter is explicitly added when launching the `yarprobotinterface`. 90 | 91 | # Credits 92 | The original iCub force control people: Matteo Fumagalli, Serena Ivaldi, Marco Randazzo, Francesco Nori. 93 | 94 | The people working on estimation during the CoDyCo project : Silvio Traversaro, Marco Randazzo, Daniele Pucci, Francesco Romano, Andrea Del Prete, Jorhabib Eljaik, Francisco Javier Andrade Chavez, Nuno Guedelha, Francesco Nori. 95 | 96 | # Citations 97 | [1] : http://ieeexplore.ieee.org/abstract/document/6100813/ 98 | 99 | [2] : http://ieeexplore.ieee.org/abstract/document/5379525/ -------------------------------------------------------------------------------- /cmake/AddInstallRPATHSupport.cmake: -------------------------------------------------------------------------------- 1 | #.rst: 2 | # AddInstallRPATHSupport 3 | # ---------------------- 4 | # 5 | # Add support to RPATH during installation to your project:: 6 | # 7 | # add_install_rpath_support([BIN_DIRS dir [dir]] 8 | # [LIB_DIRS dir [dir]] 9 | # [INSTALL_NAME_DIR [dir]] 10 | # [DEPENDS condition [condition]] 11 | # [USE_LINK_PATH]) 12 | # 13 | # Normally (depending on the platform) when you install a shared 14 | # library you can either specify its absolute path as the install name, 15 | # or leave just the library name itself. In the former case the library 16 | # will be correctly linked during run time by all executables and other 17 | # shared libraries, but it must not change its install location. This 18 | # is often the case for libraries installed in the system default 19 | # library directory (e.g. ``/usr/lib``). 20 | # In the latter case, instead, the library can be moved anywhere in the 21 | # file system but at run time the dynamic linker must be able to find 22 | # it. This is often accomplished by setting environmental variables 23 | # (i.e. ``LD_LIBRARY_PATH`` on Linux). 24 | # This procedure is usually not desirable for two main reasons: 25 | # 26 | # - by setting the variable you are changing the default behaviour 27 | # of the dynamic linker thus potentially breaking executables (not as 28 | # destructive as ``LD_PRELOAD``) 29 | # - the variable will be used only by applications spawned by the shell 30 | # and not by other processes. 31 | # 32 | # RPATH aims in solving the issues introduced by the second 33 | # installation method. Using run-path dependent libraries you can 34 | # create a directory structure containing executables and dependent 35 | # libraries that users can relocate without breaking it. 36 | # A run-path dependent library is a dependent library whose complete 37 | # install name is not known when the library is created. 38 | # Instead, the library specifies that the dynamic loader must resolve 39 | # the library’s install name when it loads the executable that depends 40 | # on the library. The executable or the other shared library will 41 | # hardcode in the binary itself the additional search directories 42 | # to be passed to the dynamic linker. This works great in conjunction 43 | # with relative paths. 44 | # This command will enable support to RPATH to your project. 45 | # It will enable the following things: 46 | # 47 | # - If the project builds shared libraries it will generate a run-path 48 | # enabled shared library, i.e. its install name will be resolved 49 | # only at run time. 50 | # - In all cases (building executables and/or shared libraries) 51 | # dependent shared libraries with RPATH support will have their name 52 | # resolved only at run time, by embedding the search path directly 53 | # into the built binary. 54 | # 55 | # The command has the following parameters: 56 | # 57 | # Options: 58 | # - ``USE_LINK_PATH``: if passed the command will automatically adds to 59 | # the RPATH the path to all the dependent libraries. 60 | # 61 | # Arguments: 62 | # - ``BIN_DIRS`` list of directories when the targets (executable and 63 | # plugins) will be installed. 64 | # - ``LIB_DIRS`` list of directories to be added to the RPATH. These 65 | # directories will be added "relative" w.r.t. the ``BIN_DIRS`` and 66 | # ``LIB_DIRS``. 67 | # - ``INSTALL_NAME_DIR`` directory where the libraries will be installed. 68 | # This variable will be used only if ``CMAKE_SKIP_RPATH`` or 69 | # ``CMAKE_SKIP_INSTALL_RPATH`` is set to ``TRUE`` as it will set the 70 | # ``INSTALL_NAME_DIR`` on all targets 71 | # - ``DEPENDS`` list of conditions that should be ``TRUE`` to enable 72 | # RPATH, for example ``FOO; NOT BAR``. 73 | # 74 | # Note: see https://gitlab.kitware.com/cmake/cmake/issues/16589 for further 75 | # details. 76 | 77 | #======================================================================= 78 | # Copyright 2014 Istituto Italiano di Tecnologia (IIT) 79 | # @author Francesco Romano 80 | # 81 | # Distributed under the OSI-approved BSD License (the "License"); 82 | # see accompanying file Copyright.txt for details. 83 | # 84 | # This software is distributed WITHOUT ANY WARRANTY; without even the 85 | # implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 86 | # See the License for more information. 87 | #======================================================================= 88 | # (To distribute this file outside of CMake, substitute the full 89 | # License text for the above reference.) 90 | 91 | 92 | include(CMakeParseArguments) 93 | 94 | 95 | function(ADD_INSTALL_RPATH_SUPPORT) 96 | 97 | set(_options USE_LINK_PATH) 98 | set(_oneValueArgs INSTALL_NAME_DIR) 99 | set(_multiValueArgs BIN_DIRS 100 | LIB_DIRS 101 | DEPENDS) 102 | 103 | cmake_parse_arguments(_ARS "${_options}" 104 | "${_oneValueArgs}" 105 | "${_multiValueArgs}" 106 | "${ARGN}") 107 | 108 | # if either RPATH or INSTALL_RPATH is disabled 109 | # and the INSTALL_NAME_DIR variable is set, then hardcode the install name 110 | if(CMAKE_SKIP_RPATH OR CMAKE_SKIP_INSTALL_RPATH) 111 | if(DEFINED _ARS_INSTALL_NAME_DIR) 112 | set(CMAKE_INSTALL_NAME_DIR ${_ARS_INSTALL_NAME_DIR} PARENT_SCOPE) 113 | endif() 114 | endif() 115 | 116 | if (CMAKE_SKIP_RPATH OR (CMAKE_SKIP_INSTALL_RPATH AND CMAKE_SKIP_BUILD_RPATH)) 117 | return() 118 | endif() 119 | 120 | 121 | set(_rpath_available 1) 122 | if(DEFINED _ARS_DEPENDS) 123 | foreach(_dep ${_ARS_DEPENDS}) 124 | string(REGEX REPLACE " +" ";" _dep "${_dep}") 125 | if(NOT (${_dep})) 126 | set(_rpath_available 0) 127 | endif() 128 | endforeach() 129 | endif() 130 | 131 | if(_rpath_available) 132 | 133 | # Enable RPATH on OSX. 134 | set(CMAKE_MACOSX_RPATH TRUE PARENT_SCOPE) 135 | 136 | # Find system implicit lib directories 137 | set(_system_lib_dirs ${CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES}) 138 | if(EXISTS "/etc/debian_version") # is this a debian system ? 139 | if(CMAKE_LIBRARY_ARCHITECTURE) 140 | list(APPEND _system_lib_dirs "/lib/${CMAKE_LIBRARY_ARCHITECTURE}" 141 | "/usr/lib/${CMAKE_LIBRARY_ARCHITECTURE}") 142 | endif() 143 | endif() 144 | # This is relative RPATH for libraries built in the same project 145 | foreach(lib_dir ${_ARS_LIB_DIRS}) 146 | list(FIND _system_lib_dirs "${lib_dir}" isSystemDir) 147 | if("${isSystemDir}" STREQUAL "-1") 148 | foreach(bin_dir ${_ARS_LIB_DIRS} ${_ARS_BIN_DIRS}) 149 | file(RELATIVE_PATH _rel_path ${bin_dir} ${lib_dir}) 150 | if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin") 151 | list(APPEND CMAKE_INSTALL_RPATH "@loader_path/${_rel_path}") 152 | else() 153 | list(APPEND CMAKE_INSTALL_RPATH "$ORIGIN/${_rel_path}") 154 | endif() 155 | endforeach() 156 | endif() 157 | endforeach() 158 | if(NOT "${CMAKE_INSTALL_RPATH}" STREQUAL "") 159 | list(REMOVE_DUPLICATES CMAKE_INSTALL_RPATH) 160 | endif() 161 | set(CMAKE_INSTALL_RPATH ${CMAKE_INSTALL_RPATH} PARENT_SCOPE) 162 | 163 | # add the automatically determined parts of the RPATH 164 | # which point to directories outside the build tree to the install RPATH 165 | set(CMAKE_INSTALL_RPATH_USE_LINK_PATH ${_ARS_USE_LINK_PATH} PARENT_SCOPE) 166 | 167 | endif() 168 | 169 | endfunction() 170 | 171 | -------------------------------------------------------------------------------- /devices/virtualAnalogClient/VirtualAnalogClient.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016 Fondazione Istituto Italiano di Tecnologia 3 | * Author: Silvio Traversaro 4 | * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT 5 | */ 6 | 7 | #include "VirtualAnalogClient.h" 8 | #include 9 | #include 10 | 11 | using namespace yarp::dev; 12 | using namespace yarp::os; 13 | using namespace yarp::sig; 14 | 15 | VirtualAnalogClient::VirtualAnalogClient() 16 | { 17 | 18 | } 19 | 20 | VirtualAnalogClient::~VirtualAnalogClient() 21 | { 22 | 23 | } 24 | 25 | bool VirtualAnalogClient::open(Searchable& config) 26 | { 27 | yarp::os::Property prop; 28 | prop.fromString(config.toString()); 29 | 30 | if( !( prop.check("local") && prop.find("local").isString() ) ) 31 | { 32 | yError("VirtualAnalogClient: Missing required local string parameter"); 33 | return false; 34 | } 35 | 36 | m_local = prop.find("local").asString(); 37 | 38 | if( !( prop.check("remote") && prop.find("remote").isString() ) ) 39 | { 40 | yError("VirtualAnalogClient: Missing required remote string parameter"); 41 | return false; 42 | } 43 | 44 | m_remote = prop.find("remote").asString(); 45 | 46 | std::string carrier; 47 | if( ( prop.check("carrier") && prop.find("carrier").isString() ) ) 48 | { 49 | carrier = prop.find("carrier").asString(); 50 | } 51 | else 52 | { 53 | carrier = "tcp"; 54 | } 55 | 56 | if( !( prop.check("AxisName") && prop.find("AxisName").isList() ) ) 57 | { 58 | yError("VirtualAnalogClient: Missing required AxisName list parameter"); 59 | return false; 60 | } 61 | 62 | Bottle * AxisNameBot = prop.find("AxisName").asList(); 63 | 64 | m_axisName.resize(AxisNameBot->size()); 65 | for(int jnt=0; jnt < AxisNameBot->size(); jnt++) 66 | { 67 | m_axisName[jnt] = AxisNameBot->get(jnt).asString(); 68 | } 69 | 70 | // Handle type 71 | m_axisType.resize(m_axisName.size()); 72 | 73 | if( ( prop.check("AxisType") && prop.find("AxisType").isList() ) ) 74 | { 75 | Bottle * AxisTypeBot = prop.find("AxisType").asList(); 76 | for(int jnt=0; jnt < AxisTypeBot->size(); jnt++) 77 | { 78 | std::string type = AxisTypeBot->get(jnt).asString(); 79 | if (type == "revolute") 80 | { 81 | m_axisType[jnt] = VOCAB_JOINTTYPE_REVOLUTE; 82 | } 83 | else if (type == "prismatic") 84 | { 85 | m_axisType[jnt] = VOCAB_JOINTTYPE_UNKNOWN; 86 | } 87 | else 88 | { 89 | yError() << "VirtualAnalogClient: unknown joint type " << type; 90 | return false; 91 | } 92 | } 93 | } 94 | else 95 | { 96 | for(size_t jnt=0; jnt < m_axisType.size(); jnt++) 97 | { 98 | m_axisType[jnt] = VOCAB_JOINTTYPE_REVOLUTE; 99 | } 100 | } 101 | 102 | if( !( prop.check("virtualAnalogSensorInteger") && prop.find("virtualAnalogSensorInteger").isInt32() ) ) 103 | { 104 | yError("VirtualAnalogClient: Missing required virtualAnalogSensorInteger int parameter"); 105 | return false; 106 | } 107 | 108 | m_virtualAnalogSensorInteger = prop.find("virtualAnalogSensorInteger").isInt32(); 109 | 110 | // Resize buffer 111 | measureBuffer.resize(m_axisName.size(),0.0); 112 | 113 | // Open the port 114 | bool ok = m_outputPort.open(m_local); 115 | 116 | if( !ok ) 117 | { 118 | yError() << "VirtualAnalogClient: impossible to open port " << m_local; 119 | } 120 | 121 | // Connect the port 122 | bool autoconnect = true; 123 | 124 | if( prop.check("autoconnect") ) 125 | { 126 | if( prop.find("autoconnect").isBool() ) 127 | { 128 | autoconnect = prop.find("autoconnect").asBool(); 129 | } 130 | else 131 | { 132 | yError() << "VirtualAnalogClient: autoconnect option found, but is not a bool, exiting."; 133 | } 134 | } 135 | 136 | if( autoconnect ) 137 | { 138 | ok = Network::connect(m_local,m_remote,carrier); 139 | 140 | if( !ok ) 141 | { 142 | yError() << "VirtualAnalogClient: impossible to connect port " << m_local << " to " << m_remote << " with carrier " << carrier; 143 | return false; 144 | } 145 | } 146 | 147 | return true; 148 | } 149 | 150 | bool VirtualAnalogClient::close() 151 | { 152 | bool ok = Network::disconnect(m_local,m_remote); 153 | m_outputPort.close(); 154 | return ok; 155 | } 156 | 157 | bool VirtualAnalogClient::updateVirtualAnalogSensorMeasure(Vector& measure) 158 | { 159 | if( (int) measure.size() != this->getVirtualAnalogSensorChannels() ) 160 | { 161 | yError() << "VirtualAnalogClient: updateMeasure failed : input measure has size " << measure.size() << " while the client is configured with " << this->getVirtualAnalogSensorChannels() << " channels"; 162 | return false; 163 | } 164 | 165 | measureBuffer = measure; 166 | 167 | sendData(); 168 | 169 | return true; 170 | } 171 | 172 | bool VirtualAnalogClient::updateVirtualAnalogSensorMeasure(int ch, double& measure) 173 | { 174 | if( ch < 0 || ch >= this->getVirtualAnalogSensorChannels() ) 175 | { 176 | yError() << "VirtualAnalogClient: updateMeasure failed : requested channel " << ch << " while the client is configured with " << this->getVirtualAnalogSensorChannels() << " channels"; 177 | } 178 | 179 | measureBuffer[ch] = measure; 180 | 181 | sendData(); 182 | 183 | return true; 184 | } 185 | 186 | void VirtualAnalogClient::sendData() 187 | { 188 | Bottle & a = m_outputPort.prepare(); 189 | a.clear(); 190 | a.addInt32(m_virtualAnalogSensorInteger); 191 | for(size_t i=0;im_axisName.size(); 201 | } 202 | 203 | VAS_status VirtualAnalogClient::getVirtualAnalogSensorStatus(int /*ch*/) 204 | { 205 | return yarp::dev::VAS_status::VAS_OK; 206 | } 207 | 208 | bool VirtualAnalogClient::getAxisName(int axis, std::string& name) 209 | { 210 | if( axis < 0 || axis >= this->getVirtualAnalogSensorChannels() ) 211 | { 212 | yError() << "VirtualAnalogClient: getAxisName failed : requested axis " << axis << " while the client is configured with " << this->getVirtualAnalogSensorChannels() << " channels"; 213 | return false; 214 | } 215 | 216 | name = m_axisName[axis]; 217 | 218 | return true; 219 | } 220 | 221 | bool VirtualAnalogClient::getJointType(int axis, JointTypeEnum& type) 222 | { 223 | if( axis < 0 || axis >= this->getVirtualAnalogSensorChannels() ) 224 | { 225 | yError() << "VirtualAnalogClient: getJointType failed : requested axis " << axis << " while the client is configured with " << this->getVirtualAnalogSensorChannels() << " channels"; 226 | return false; 227 | } 228 | 229 | type = m_axisType[axis]; 230 | return true; 231 | } 232 | 233 | bool VirtualAnalogClient::getAxes(int* ax) 234 | { 235 | if( !ax ) 236 | { 237 | yError() << "VirtualAnalogClient: getAxes failed : invalid argument passed"; 238 | return false; 239 | } 240 | 241 | *ax = VirtualAnalogClient::getVirtualAnalogSensorChannels(); 242 | return true; 243 | } 244 | -------------------------------------------------------------------------------- /libraries/ctrlLibRT/src/filters.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 RobotCub Consortium, European Commission FP6 Project IST-004370 3 | * Author: Silvio Traversaro 4 | * email: silvio.traversaro@iit.it 5 | * website: www.icub.org 6 | * Permission is granted to copy, distribute, and/or modify this program 7 | * under the terms of the GNU General Public License, version 2 or any 8 | * later version published by the Free Software Foundation. 9 | * 10 | * A copy of the license can be found at 11 | * http://www.robotcub.org/icub/license/gpl.txt 12 | * 13 | * This program is distributed in the hope that it will be useful, but 14 | * WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General 16 | * Public License for more details 17 | */ 18 | 19 | #include "ctrlLibRT/filters.h" 20 | 21 | #include 22 | 23 | using namespace std; 24 | using namespace yarp::sig; 25 | using namespace yarp::math; 26 | using namespace iCub::ctrl::realTime; 27 | 28 | /***************************************************************************/ 29 | Eigen::Map toEigen(yarp::sig::Vector & vec) 30 | { 31 | return Eigen::Map(vec.data(),vec.size()); 32 | } 33 | 34 | Eigen::Map toEigen(const yarp::sig::Vector & vec) 35 | { 36 | return Eigen::Map(vec.data(),vec.size()); 37 | } 38 | 39 | /***************************************************************************/ 40 | Filter::Filter(const Vector &num, const Vector &den, const Vector &y0) 41 | { 42 | b=toEigen(num); 43 | a=toEigen(den); 44 | 45 | m=b.size(); uold.resize(y0.size(),m-1); 46 | n=a.size(); yold.resize(y0.size(),m-1); 47 | 48 | init(y0); 49 | } 50 | 51 | 52 | /***************************************************************************/ 53 | void Filter::init(const Vector &y0) 54 | { 55 | // otherwise use zero 56 | init(y0,yarp::math::zeros(y0.length())); 57 | } 58 | 59 | 60 | /***************************************************************************/ 61 | void Filter::init(const Vector &y0, const Vector &u0) 62 | { 63 | Vector u_init(y0.length(),0.0); 64 | Vector y_init=y0; 65 | y=y0; 66 | 67 | double sum_b=0.0; 68 | for (int i=0; i1e-9) // if filter DC gain is not zero 76 | u_init=(sum_a/sum_b)*y0; 77 | else 78 | { 79 | // if filter gain is zero then you need to know in advance what 80 | // the next input is going to be for initializing (that is u0) 81 | // Note that, unless y0=0, the filter output is not going to be stable 82 | u_init=u0; 83 | if (fabs(sum_a-a[0])>1e-9) 84 | y_init=a[0]/(a[0]-sum_a)*y; 85 | // if sum_a==a[0] then the filter can only be initialized to zero 86 | } 87 | 88 | for (int i=0; iinit(y0); 191 | } 192 | 193 | 194 | /**********************************************************************/ 195 | bool FirstOrderLowPassFilter::setCutFrequency(const double cutFrequency) 196 | { 197 | if (cutFrequency<=0.0) 198 | return false; 199 | 200 | // Optimization : if the cutoff frequency is exactly the same 201 | // do not update the coeffiencts 202 | // Note: in general the equality between two doubles is not a 203 | // reliable check, but in this case it make sense 204 | if( fc != cutFrequency ) 205 | { 206 | fc=cutFrequency; 207 | computeCoeff(); 208 | } 209 | 210 | return true; 211 | } 212 | 213 | 214 | /**********************************************************************/ 215 | bool FirstOrderLowPassFilter::setSampleTime(const double sampleTime) 216 | { 217 | if (sampleTime<=0.0) 218 | return false; 219 | 220 | Ts=sampleTime; 221 | computeCoeff(); 222 | 223 | return true; 224 | } 225 | 226 | 227 | /**********************************************************************/ 228 | const Vector& FirstOrderLowPassFilter::filt(const Vector &u) 229 | { 230 | if (filter!=NULL) 231 | y=filter->filt(u); 232 | 233 | return y; 234 | } 235 | 236 | 237 | /**********************************************************************/ 238 | void FirstOrderLowPassFilter::computeCoeff() 239 | { 240 | double tau=1.0/(2.0*M_PI*fc); 241 | Vector num=cat(Ts,Ts); 242 | Vector den=cat(2.0*tau+Ts,Ts-2.0*tau); 243 | 244 | if (filter!=NULL) 245 | filter->adjustCoeffs(num,den); 246 | else 247 | filter=new Filter(num,den,y); 248 | } 249 | 250 | -------------------------------------------------------------------------------- /devices/wholeBodyDynamics/GravityCompensationHelpers.cpp: -------------------------------------------------------------------------------- 1 | #include "GravityCompensationHelpers.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | using namespace iDynTree; 11 | 12 | namespace wholeBodyDynamics 13 | { 14 | 15 | GravityCompensationHelper::GravityCompensationHelper(): m_model(), 16 | m_isModelValid(false), 17 | m_isKinematicsUpdated(false), 18 | m_dynamicTraversal(), 19 | m_kinematicTraversals(), 20 | m_jointPos(), 21 | m_linkVels(), 22 | m_linkProperAccs(), 23 | m_linkIntWrenches(), 24 | m_linkNetExternalWrenchesZero(), 25 | m_generalizedTorques() 26 | { 27 | 28 | } 29 | 30 | GravityCompensationHelper::~GravityCompensationHelper() 31 | { 32 | freeKinematicTraversals(); 33 | } 34 | 35 | 36 | void GravityCompensationHelper::allocKinematicTraversals(const size_t nrOfLinks) 37 | { 38 | m_kinematicTraversals.resize(nrOfLinks); 39 | for(size_t link=0; link < m_kinematicTraversals.size(); link++) 40 | { 41 | m_kinematicTraversals[link] = new Traversal(); 42 | } 43 | } 44 | 45 | void GravityCompensationHelper::freeKinematicTraversals() 46 | { 47 | for(size_t link=0; link < m_kinematicTraversals.size(); link++) 48 | { 49 | delete m_kinematicTraversals[link]; 50 | } 51 | m_kinematicTraversals.resize(0); 52 | } 53 | 54 | bool GravityCompensationHelper::loadModel(const Model& _model, const std::string dynamicBase) 55 | { 56 | m_model = _model; 57 | 58 | // resize the data structures 59 | iDynTree::LinkIndex dynamicBaseIndex = m_model.getLinkIndex(dynamicBase); 60 | 61 | bool ok = m_model.computeFullTreeTraversal(m_dynamicTraversal,dynamicBaseIndex); 62 | 63 | if( !ok ) 64 | { 65 | m_isModelValid = false; 66 | return false; 67 | } 68 | 69 | freeKinematicTraversals(); 70 | allocKinematicTraversals(m_model.getNrOfLinks()); 71 | 72 | m_jointPos.resize(m_model); 73 | m_jointDofsZero.resize(m_model); 74 | m_jointDofsZero.zero(); 75 | m_linkVels.resize(m_model); 76 | m_linkProperAccs.resize(m_model); 77 | m_linkIntWrenches.resize(m_model); 78 | m_linkNetExternalWrenchesZero.resize(m_model); 79 | m_generalizedTorques.resize(m_model); 80 | 81 | // set that the model is valid 82 | m_isModelValid = true; 83 | 84 | return true; 85 | } 86 | 87 | 88 | bool GravityCompensationHelper::updateKinematicsFromGravity(const JointPosDoubleArray& jointPos, 89 | const FrameIndex& fixedFrame, 90 | const Vector3& gravity) 91 | { 92 | if( !m_isModelValid ) 93 | { 94 | reportError("GravityCompensationHelper","updateKinematicsFromGravity","Model and sensors information not setted."); 95 | return false; 96 | } 97 | 98 | Vector3 zero; 99 | zero.zero(); 100 | 101 | Vector3 properClassicalAcceleration; 102 | 103 | properClassicalAcceleration(0) = -gravity(0); 104 | properClassicalAcceleration(1) = -gravity(1); 105 | properClassicalAcceleration(2) = -gravity(2); 106 | 107 | return updateKinematicsFromProperAcceleration(jointPos,fixedFrame,properClassicalAcceleration); 108 | } 109 | 110 | bool GravityCompensationHelper::updateKinematicsFromProperAcceleration(const JointPosDoubleArray& jointPos, 111 | const FrameIndex& floatingFrame, 112 | const Vector3& properClassicalLinearAcceleration) 113 | { 114 | if( !m_isModelValid ) 115 | { 116 | reportError("GravityCompensationHelper","updateKinematicsFromProperAcceleration","Model information not setted."); 117 | return false; 118 | } 119 | 120 | if( floatingFrame == FRAME_INVALID_INDEX || 121 | floatingFrame < 0 || floatingFrame >= m_model.getNrOfFrames() ) 122 | { 123 | reportError("ExtWrenchesAndJointTorquesEstimator","updateKinematicsFromProperAcceleration","Unknown frame index specified."); 124 | return false; 125 | } 126 | 127 | // Get link of the specified frame 128 | LinkIndex floatingLinkIndex = m_model.getFrameLink(floatingFrame); 129 | 130 | // Build the traversal if it is not present 131 | if( m_kinematicTraversals[floatingLinkIndex]->getNrOfVisitedLinks() == 0 ) 132 | { 133 | m_model.computeFullTreeTraversal(*m_kinematicTraversals[floatingLinkIndex],floatingLinkIndex); 134 | } 135 | 136 | // To initialize the kinematic propagation, we should first convert the kinematics 137 | // information from the frame in which they are specified to the main frame of the link 138 | Transform link_H_frame = m_model.getFrameTransform(floatingFrame); 139 | 140 | Twist base_vel_frame, base_vel_link; 141 | SpatialAcc base_acc_frame, base_acc_link; 142 | ClassicalAcc base_classical_acc_link; 143 | 144 | Vector3 zero3; 145 | zero3.zero(); 146 | 147 | base_vel_frame.setLinearVec3(zero3); 148 | base_vel_frame.setAngularVec3(zero3); 149 | 150 | base_vel_link = link_H_frame*base_vel_frame; 151 | 152 | base_acc_frame.setLinearVec3(properClassicalLinearAcceleration); 153 | base_acc_frame.setAngularVec3(zero3); 154 | 155 | base_acc_link = link_H_frame*base_acc_frame; 156 | 157 | base_classical_acc_link.fromSpatial(base_acc_link,base_vel_link); 158 | 159 | // Propagate the kinematics information 160 | bool ok = dynamicsEstimationForwardVelAccKinematics(m_model,*(m_kinematicTraversals[floatingLinkIndex]), 161 | base_classical_acc_link.getLinearVec3(), 162 | base_vel_link.getAngularVec3(), 163 | base_classical_acc_link.getAngularVec3(), 164 | jointPos,m_jointDofsZero,m_jointDofsZero, 165 | m_linkVels,m_linkProperAccs); 166 | 167 | // Store joint positions 168 | m_jointPos = jointPos; 169 | 170 | if( !ok ) 171 | { 172 | return false; 173 | } 174 | else 175 | { 176 | m_isKinematicsUpdated = true; 177 | return true; 178 | } 179 | } 180 | 181 | bool GravityCompensationHelper::getGravityCompensationTorques(JointDOFsDoubleArray & jointTrqs) 182 | { 183 | if( !m_isModelValid ) 184 | { 185 | reportError("GravityCompensationHelper","getGravityCompensationTorques", 186 | "Model not set."); 187 | return false; 188 | } 189 | 190 | if( !m_isKinematicsUpdated ) 191 | { 192 | reportError("GravityCompensationHelper","getGravityCompensationTorques", 193 | "Kinematic information not set."); 194 | return false; 195 | } 196 | 197 | /** 198 | * Compute joint torques 199 | */ 200 | bool ok = RNEADynamicPhase(m_model,m_dynamicTraversal,m_jointPos,m_linkVels,m_linkProperAccs, 201 | m_linkNetExternalWrenchesZero,m_linkIntWrenches,m_generalizedTorques); 202 | 203 | if( !ok ) 204 | { 205 | reportError("ExtWrenchesAndJointTorquesEstimator","estimateExtWrenchesAndJointTorques", 206 | "Error in computing the dynamic phase of the RNEA."); 207 | return false; 208 | } 209 | 210 | /** 211 | * Copy the joint torques computed by the RNEA to the output 212 | */ 213 | jointTrqs = m_generalizedTorques.jointTorques(); 214 | 215 | return true; 216 | } 217 | 218 | 219 | 220 | } 221 | 222 | 223 | --------------------------------------------------------------------------------