diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..6ca21f9 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,32 @@ +# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +cmake_minimum_required(VERSION 3.5) +project(whole-body-estimators LANGUAGES CXX + VERSION 0.1) + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON) + +list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) + +option(ENABLE_RPATH "Enable RPATH for this library" ON) +mark_as_advanced(ENABLE_RPATH) +include(AddInstallRPATHSupport) +add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_BINDIR}" + LIB_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}" + INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}" + DEPENDS ENABLE_RPATH + USE_LINK_PATH) + +set(YARP_REQUIRED_VERSION 3.0.1) + +find_package(YARP ${YARP_REQUIRED_VERSION} REQUIRED) + +add_subdirectory(devices) + +include(AddUninstallTarget) + diff --git a/README.md b/README.md index f19bcf8..22f177a 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,54 @@ # whole-body-estimators YARP-based estimators for humanoid robots. + +# Overview + - [:orange_book: Implementation](#orange_book-implementation) + - [:page_facing_up: Dependencies](#page_facing_up-dependencies) + - [:hammer: Build the suite](#hammer-build-the-suite) + +# :orange_book: Implementation +The current implementations available in the `devices` folder include, +- `baseEstimatorV1` includes a simple humanoid floating base estimation algorithm fusing legged odometry and IMU attitude estimation, aimed to be used along side walking controllers. + +![Floating Base Estimation Algorithm V1](doc/resources/fbeV1.png) + + +# :page_facing_up: Dependencies +* [YARP](http://www.yarp.it/): to handle the comunication with the robot; +* [iDynTree](https://github.com/robotology/idyntree/tree/devel): to setup the floating base estimation algorithm; +* [ICUB](https://github.com/robotology/icub-main): to use the utilities like low pass filters from the `ctrLib` library +* [Gazebo](http://gazebosim.org/): for the simulation (tested Gazebo 8 and 9). + + ## Optional Dependencies + * [walking-controllers](https://github.com/robotology/walking-controllers): to test the floating base estimation along side walking controllers + +It must be noted that all of these dependencies can be directly installed together in one place using the [robotology-superbuild](https://github.com/robotology/robotology-superbuild). + + +# :hammer: Build the suite +## Linux + +```sh +git clone https://github.com/robotology/whole-body-estimators.git +cd whole-body-estimators +mkdir build && cd build +cmake ../ +make +[sudo] make install +``` +Notice: `sudo` is not necessary if you specify the `CMAKE_INSTALL_PREFIX`. In this case it is necessary to add in the `.bashrc` or `.bash_profile` the following lines: +``` sh +export WBDEstimator_INSTALL_DIR=/path/where/you/installed +export YARP_DATA_DIRS=${YARP_DATA_DIRS}:${WBDEstimator_INSTALL_DIR}/share/yarp:${WBDEstimator_INSTALL_DIR}/lib/yarp +``` + +# Using the estimators +- [`baseEstimatorV1`](devices/baseEstimatorV1/README.md) Please follow the documentation available here to run the floating base estimator. + +# Authors +``` +Prashanth Ramadoss +Giulio Romualdi +Silvio Traversaro +Daniele Pucci +``` diff --git a/cmake/AddInstallRPATHSupport.cmake b/cmake/AddInstallRPATHSupport.cmake new file mode 100644 index 0000000..cefdd02 --- /dev/null +++ b/cmake/AddInstallRPATHSupport.cmake @@ -0,0 +1,170 @@ +#.rst: +# AddInstallRPATHSupport +# ---------------------- +# +# Add support to RPATH during installation to your project:: +# +# add_install_rpath_support([BIN_DIRS dir [dir]] +# [LIB_DIRS dir [dir]] +# [INSTALL_NAME_DIR [dir]] +# [DEPENDS condition [condition]] +# [USE_LINK_PATH]) +# +# Normally (depending on the platform) when you install a shared +# library you can either specify its absolute path as the install name, +# or leave just the library name itself. In the former case the library +# will be correctly linked during run time by all executables and other +# shared libraries, but it must not change its install location. This +# is often the case for libraries installed in the system default +# library directory (e.g. ``/usr/lib``). +# In the latter case, instead, the library can be moved anywhere in the +# file system but at run time the dynamic linker must be able to find +# it. This is often accomplished by setting environmental variables +# (i.e. ``LD_LIBRARY_PATH`` on Linux). +# This procedure is usually not desirable for two main reasons: +# +# - by setting the variable you are changing the default behaviour +# of the dynamic linker thus potentially breaking executables (not as +# destructive as ``LD_PRELOAD``) +# - the variable will be used only by applications spawned by the shell +# and not by other processes. +# +# RPATH aims in solving the issues introduced by the second +# installation method. Using run-path dependent libraries you can +# create a directory structure containing executables and dependent +# libraries that users can relocate without breaking it. +# A run-path dependent library is a dependent library whose complete +# install name is not known when the library is created. +# Instead, the library specifies that the dynamic loader must resolve +# the library’s install name when it loads the executable that depends +# on the library. The executable or the other shared library will +# hardcode in the binary itself the additional search directories +# to be passed to the dynamic linker. This works great in conjunction +# with relative paths. +# This command will enable support to RPATH to your project. +# It will enable the following things: +# +# - If the project builds shared libraries it will generate a run-path +# enabled shared library, i.e. its install name will be resolved +# only at run time. +# - In all cases (building executables and/or shared libraries) +# dependent shared libraries with RPATH support will have their name +# resolved only at run time, by embedding the search path directly +# into the built binary. +# +# The command has the following parameters: +# +# Options: +# - ``USE_LINK_PATH``: if passed the command will automatically adds to +# the RPATH the path to all the dependent libraries. +# +# Arguments: +# - ``BIN_DIRS`` list of directories when the targets (executable and +# plugins) will be installed. +# - ``LIB_DIRS`` list of directories to be added to the RPATH. These +# directories will be added "relative" w.r.t. the ``BIN_DIRS`` and +# ``LIB_DIRS``. +# - ``INSTALL_NAME_DIR`` directory where the libraries will be installed. +# This variable will be used only if ``CMAKE_SKIP_RPATH`` or +# ``CMAKE_SKIP_INSTALL_RPATH`` is set to ``TRUE`` as it will set the +# ``INSTALL_NAME_DIR`` on all targets +# - ``DEPENDS`` list of conditions that should be ``TRUE`` to enable +# RPATH, for example ``FOO; NOT BAR``. +# +# Note: see https://gitlab.kitware.com/cmake/cmake/issues/16589 for further +# details. + +#======================================================================= +# Copyright 2014 Istituto Italiano di Tecnologia (IIT) +# @author Francesco Romano +# +# Distributed under the OSI-approved BSD License (the "License"); +# see accompanying file Copyright.txt for details. +# +# This software is distributed WITHOUT ANY WARRANTY; without even the +# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +# See the License for more information. +#======================================================================= +# (To distribute this file outside of CMake, substitute the full +# License text for the above reference.) + + +include(CMakeParseArguments) + + +function(ADD_INSTALL_RPATH_SUPPORT) + + set(_options USE_LINK_PATH) + set(_oneValueArgs INSTALL_NAME_DIR) + set(_multiValueArgs BIN_DIRS + LIB_DIRS + DEPENDS) + + cmake_parse_arguments(_ARS "${_options}" + "${_oneValueArgs}" + "${_multiValueArgs}" + "${ARGN}") + + # if either RPATH or INSTALL_RPATH is disabled + # and the INSTALL_NAME_DIR variable is set, then hardcode the install name + if(CMAKE_SKIP_RPATH OR CMAKE_SKIP_INSTALL_RPATH) + if(DEFINED _ARS_INSTALL_NAME_DIR) + set(CMAKE_INSTALL_NAME_DIR ${_ARS_INSTALL_NAME_DIR} PARENT_SCOPE) + endif() + endif() + + if (CMAKE_SKIP_RPATH OR (CMAKE_SKIP_INSTALL_RPATH AND CMAKE_SKIP_BUILD_RPATH)) + return() + endif() + + + set(_rpath_available 1) + if(DEFINED _ARS_DEPENDS) + foreach(_dep ${_ARS_DEPENDS}) + string(REGEX REPLACE " +" ";" _dep "${_dep}") + if(NOT (${_dep})) + set(_rpath_available 0) + endif() + endforeach() + endif() + + if(_rpath_available) + + # Enable RPATH on OSX. + set(CMAKE_MACOSX_RPATH TRUE PARENT_SCOPE) + + # Find system implicit lib directories + set(_system_lib_dirs ${CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES}) + if(EXISTS "/etc/debian_version") # is this a debian system ? + if(CMAKE_LIBRARY_ARCHITECTURE) + list(APPEND _system_lib_dirs "/lib/${CMAKE_LIBRARY_ARCHITECTURE}" + "/usr/lib/${CMAKE_LIBRARY_ARCHITECTURE}") + endif() + endif() + # This is relative RPATH for libraries built in the same project + foreach(lib_dir ${_ARS_LIB_DIRS}) + list(FIND _system_lib_dirs "${lib_dir}" isSystemDir) + if("${isSystemDir}" STREQUAL "-1") + foreach(bin_dir ${_ARS_LIB_DIRS} ${_ARS_BIN_DIRS}) + file(RELATIVE_PATH _rel_path ${bin_dir} ${lib_dir}) + if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin") + list(APPEND CMAKE_INSTALL_RPATH "@loader_path/${_rel_path}") + else() + list(APPEND CMAKE_INSTALL_RPATH "$ORIGIN/${_rel_path}") + endif() + endforeach() + endif() + endforeach() + if(NOT "${CMAKE_INSTALL_RPATH}" STREQUAL "") + list(REMOVE_DUPLICATES CMAKE_INSTALL_RPATH) + endif() + set(CMAKE_INSTALL_RPATH ${CMAKE_INSTALL_RPATH} PARENT_SCOPE) + + # add the automatically determined parts of the RPATH + # which point to directories outside the build tree to the install RPATH + set(CMAKE_INSTALL_RPATH_USE_LINK_PATH ${_ARS_USE_LINK_PATH} PARENT_SCOPE) + + endif() + +endfunction() + diff --git a/cmake/AddUninstallTarget.cmake b/cmake/AddUninstallTarget.cmake new file mode 100644 index 0000000..f47198d --- /dev/null +++ b/cmake/AddUninstallTarget.cmake @@ -0,0 +1,102 @@ +#.rst: +# AddUninstallTarget +# ------------------ +# +# Add the "uninstall" target for your project:: +# +# include(AddUninstallTarget) +# +# +# will create a file ``cmake_uninstall.cmake`` in the build directory and add a +# custom target ``uninstall`` (or ``UNINSTALL`` on Visual Studio and Xcode) that +# will remove the files installed by your package (using +# ``install_manifest.txt``). +# See also +# https://gitlab.kitware.com/cmake/community/wikis/FAQ#can-i-do-make-uninstall-with-cmake +# +# The :module:`AddUninstallTarget` module must be included in your main +# ``CMakeLists.txt``. If included in a subdirectory it does nothing. +# This allows you to use it safely in your main ``CMakeLists.txt`` and include +# your project using ``add_subdirectory`` (for example when using it with +# :cmake:module:`FetchContent`). +# +# If the ``uninstall`` target already exists, the module does nothing. + +#============================================================================= +# Copyright 2008-2013 Kitware, Inc. +# Copyright 2013 Istituto Italiano di Tecnologia (IIT) +# Authors: Daniele E. Domenichelli +# +# Distributed under the OSI-approved BSD License (the "License"); +# see accompanying file Copyright.txt for details. +# +# This software is distributed WITHOUT ANY WARRANTY; without even the +# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +# See the License for more information. +#============================================================================= +# (To distribute this file outside of CMake, substitute the full +# License text for the above reference.) + + +# AddUninstallTarget works only when included in the main CMakeLists.txt +if(NOT "${CMAKE_CURRENT_BINARY_DIR}" STREQUAL "${CMAKE_BINARY_DIR}") + return() +endif() + +# The name of the target is uppercase in MSVC and Xcode (for coherence with the +# other standard targets) +if("${CMAKE_GENERATOR}" MATCHES "^(Visual Studio|Xcode)") + set(_uninstall "UNINSTALL") +else() + set(_uninstall "uninstall") +endif() + +# If target is already defined don't do anything +if(TARGET ${_uninstall}) + return() +endif() + + +set(_filename cmake_uninstall.cmake) + +file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/${_filename}" +"if(NOT EXISTS \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\") + message(WARNING \"Cannot find install manifest: \\\"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\\\"\") + return() +endif() +file(READ \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\" files) +string(STRIP \"${files}\" files) +string(REGEX REPLACE \"\\n\" \";\" files \"${files}\") +list(REVERSE files) +foreach(file ${files}) + if(IS_SYMLINK \"$ENV{DESTDIR}${file}\" OR EXISTS \"$ENV{DESTDIR}${file}\") + message(STATUS \"Uninstalling: $ENV{DESTDIR}${file}\") + execute_process( + COMMAND ${CMAKE_COMMAND} -E remove \"$ENV{DESTDIR}${file}\" + OUTPUT_VARIABLE rm_out + RESULT_VARIABLE rm_retval) + if(NOT \"${rm_retval}\" EQUAL 0) + message(FATAL_ERROR \"Problem when removing \\\"$ENV{DESTDIR}${file}\\\"\") + endif() + else() + message(STATUS \"Not-found: $ENV{DESTDIR}${file}\") + endif() +endforeach(file) +") + +set(_desc "Uninstall the project...") +if(CMAKE_GENERATOR STREQUAL "Unix Makefiles") + set(_comment COMMAND $\(CMAKE_COMMAND\) -E cmake_echo_color --switch=$\(COLOR\) --cyan "${_desc}") +else() + set(_comment COMMENT "${_desc}") +endif() +add_custom_target(${_uninstall} + ${_comment} + COMMAND ${CMAKE_COMMAND} -P ${_filename} + USES_TERMINAL + BYPRODUCTS uninstall_byproduct + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}") +set_property(SOURCE uninstall_byproduct PROPERTY SYMBOLIC 1) + +set_property(TARGET ${_uninstall} PROPERTY FOLDER "CMakePredefinedTargets") + diff --git a/devices/CMakeLists.txt b/devices/CMakeLists.txt new file mode 100644 index 0000000..c9baf6c --- /dev/null +++ b/devices/CMakeLists.txt @@ -0,0 +1,6 @@ +# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +add_subdirectory(baseEstimatorV1) + diff --git a/devices/baseEstimatorV1/CMakeLists.txt b/devices/baseEstimatorV1/CMakeLists.txt new file mode 100644 index 0000000..0332fbe --- /dev/null +++ b/devices/baseEstimatorV1/CMakeLists.txt @@ -0,0 +1,54 @@ +# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +set(iDynTree_REQUIRED_VERSION 0.11.0) + +find_package(iDynTree ${iDynTree_VERSION} REQUIRED) + +find_package(ICUB REQUIRED) + +set(FBE_HEADERS ${CMAKE_CURRENT_SOURCE_DIR}/include/baseEstimatorV1.h + ${CMAKE_CURRENT_SOURCE_DIR}/include/WalkingLogger.tpp + ${CMAKE_CURRENT_SOURCE_DIR}/include/WalkingLogger.hpp + ${CMAKE_CURRENT_SOURCE_DIR}/include/Utils.tpp + ${CMAKE_CURRENT_SOURCE_DIR}/include/Utils.hpp) + +set(FBE_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/baseEstimatorV1.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/fbeRobotInterface.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/configureEstimator.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/WalkingLogger.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/Utils.cpp) + +yarp_prepare_plugin(baseEstimatorV1 CATEGORY device + TYPE yarp::dev::baseEstimatorV1 + INCLUDE ${FBE_HEADERS} + DEFAULT ON) + +yarp_add_idl(THRIFT "${CMAKE_CURRENT_SOURCE_DIR}/thrifts/floatingBaseEstimationRPC.thrift") +add_library(floatingBaseEstimationRPC-service STATIC ${THRIFT}) +target_include_directories(floatingBaseEstimationRPC-service PUBLIC ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_BINDIR}/include) +target_link_libraries(floatingBaseEstimationRPC-service YARP::YARP_init YARP::YARP_OS) +set_property(TARGET floatingBaseEstimationRPC-service PROPERTY POSITION_INDEPENDENT_CODE ON) + +yarp_add_plugin(baseEstimatorV1 ${FBE_SOURCES} ${FBE_HEADERS}) + +target_include_directories(baseEstimatorV1 PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/include + ${Eigen3_INCLUDE_DIRS}) + +target_link_libraries(baseEstimatorV1 ${YARP_LIBRARIES} + ${iDynTree_LIBRARIES} + floatingBaseEstimationRPC-service) + +yarp_install(TARGETS baseEstimatorV1 + COMPONENT Runtime + LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR}/ + ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR}/) + +yarp_install(FILES baseEstimatorV1.ini + COMPONENT runtime + DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}/) + +add_subdirectory(app) + diff --git a/devices/baseEstimatorV1/README.md b/devices/baseEstimatorV1/README.md new file mode 100644 index 0000000..abb8247 --- /dev/null +++ b/devices/baseEstimatorV1/README.md @@ -0,0 +1,72 @@ +# Overview +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. + +- [:computer: How to run the simulation](#computer-how-to-run-the-simulation) +- [:running: How to test on iCub](#running-how-to-test-on-icub) + +# :computer: How to run the simulation +1. Set the `YARP_ROBOT_NAME` environment variable according to the chosen Gazebo model: + ```sh + export YARP_ROBOT_NAME="iCubGazeboV2_5" + ``` +2. Run `yarpserver` + ``` sh + yarpserver --write + ``` +3. Run gazebo and drag and drop iCub (e.g. icubGazeboSim or iCubGazeboV2_5): + + ``` sh + gazebo -slibgazebo_yarp_clock.so + ``` +4. Run `yarprobotinterface` with corresponding device configuration file. + - To run `baseEstimatorV1`, + + ``` sh + YARP_CLOCK=/clock yarprobotinterface --config launch-fbe-analogsens.xml + ``` + This launches both the floating base estimation device and the whole body dynamics device. +5. Reset the offset of the FT sensors. Open a terminal and write + + ``` + yarp rpc /wholeBodyDynamics/rpc + >> resetOffset all + ``` + +6. communicate with the `base-estimator` through RPC service calls: + ``` + yarp rpc /base-estimator/rpc + ``` + the following commands are allowed: + * `startFloatingBaseFilter`: starts the estimator, this needs to be run after the FT sensors are reset; + + other optional commands include, + + * `setContactSchmittThreshold lbreak lmake rbreak rmake`: used to set contact force thresholds for feet contact detection; + * `setPrimaryFoot foot`: set the foot to the one that does not break the contact initially during walking, `foot` can be `left` or `right`; + * `useJointVelocityLPF flag`: use a low pass filter on the joint velocities, `flag` can be `true` or `false`; + * `setJointVelocityLPFCutoffFrequency freq`: set the cut-off frequency for the low pass filter on the joint velocities; + * `resetLeggedOdometry`: reset the floating base pose and reset the legged odometry to the inital state; + * `resetLeggedOdometryWithRefFrame frame x y z roll pitch yaw`: reset the legged odometry by mentioning an intial reference to the world frame with respect to the initial fixed frame; + * `getRefFrameForWorld`: get the initial fixed frame with respect to which the world frame was set; + +## Configuration + +The configuration file for the estimator can be found `app/robots/${YARP_ROBOT_NAME}/fbe-analogsens.xml`. +The attitude estimation for the head IMU can be chosen to be either a QuaternionEKF or a non-linear complementary filter. The gains should be tuned accordingly. + +## How to dump data +Before run `yarprobotinterface` check if [`dump_data`](app/robots/iCubGazeboV2_5/fbe-analogsens.xml#L14) is set to `true` + +If `true`, run the Logger Module +``` sh +YARP_CLOCK=/clock WalkingLoggerModule +``` + +All the data will be saved in the current folder inside a `txt` named `Dataset_YYYY_MM_DD_HH_MM_SS.txt` + +# :running: How to test on iCub +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) +## :warning: Warning +Currently the supported robots are only: +- ``iCubGenova04`` +- ``icubGazeboV2_5`` diff --git a/devices/baseEstimatorV1/app/CMakeLists.txt b/devices/baseEstimatorV1/app/CMakeLists.txt new file mode 100644 index 0000000..bfd8a52 --- /dev/null +++ b/devices/baseEstimatorV1/app/CMakeLists.txt @@ -0,0 +1,25 @@ +# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +# Thanks to Stefano Dafarra for this CMakeLists.txt + +# List the subdirectory (http://stackoverflow.com/questions/7787823/cmake-how-to-get-the-name-of-all-subdirectories-of-a-directory) +macro(SUBDIRLIST result curdir) + file(GLOB children RELATIVE ${curdir} ${curdir}/*) + set(dirlist "") + foreach(child ${children}) + if(IS_DIRECTORY ${curdir}/${child}) + list(APPEND dirlist ${child}) + endif() + endforeach() + set(${result} ${dirlist}) +endmacro() + +# Get list of models +subdirlist(subdirs ${CMAKE_CURRENT_SOURCE_DIR}/robots/) +# Install each model +foreach (dir ${subdirs}) + yarp_install(DIRECTORY robots/${dir} DESTINATION ${YARP_ROBOTS_INSTALL_DIR}) +endforeach () + diff --git a/devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/fbe-analogsens.xml b/devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/fbe-analogsens.xml new file mode 100644 index 0000000..2a24309 --- /dev/null +++ b/devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/fbe-analogsens.xml @@ -0,0 +1,97 @@ + + + + + + model.urdf + icubSim + 0.010 + ("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") + root_link + l_foot_ft_sensor + r_foot_ft_sensor + + false + false + false + 10.0 + + + l_sole + l_sole + (0 0 0 0 0 0) + + + qekf + (1.0 0.0 0.0 0.0) + 0.0 + 0.0 + + + 0.010 + 0.7 + 0.001 + false + + head_imu_acc_1x1 + (0 0 0) + + + 0.010 + 0.03 + 0.0 + 0.5 + 10e-11 + 10e-6 + 10e-1 + 10e-11 + 10e-3 + false + + + right + 1.0 + 1.0 + 130.0 + 25.0 + 150.0 + 25.0 + + + /wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o + /wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o + + + /logger/data:o + /logger/rpc:o + /logger/data:i + /logger/rpc:i + + + + + all_joints_mc + + inertial + + + left_upper_arm_strain + right_upper_arm_strain + left_upper_leg_strain + right_upper_leg_strain + left_lower_leg_strain + right_lower_leg_strain + + + + + + + + diff --git a/devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/launch-fbe-analogsens.xml b/devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/launch-fbe-analogsens.xml new file mode 100644 index 0000000..254cba6 --- /dev/null +++ b/devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/launch-fbe-analogsens.xml @@ -0,0 +1,92 @@ + + + + + + + /icubSim/torso + /baseestimation/torso + + + /icubSim/left_arm + /baseestimation/left_arm + + + /icubSim/right_arm + /baseestimation/right_arm + + + /icubSim/left_leg + /baseestimation/left_leg + + + /icubSim/right_leg + /baseestimation/right_leg + + + /icubSim/head + /baseestimation/head + + + + + ("/icubSim/head", "/icubSim/torso", "/icubSim/left_arm", "/icubSim/right_arm", "/icubSim/left_leg", "/icubSim/right_leg") + ("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") + /baseestimation + + + + + /icubSim/inertial + /baseestimation/head/imu:i + + + + + /icubSim/left_arm/analog:o + /baseestimation/l_arm_ft_sensor + + + + /icubSim/right_arm/analog:o + /baseestimation/r_arm_ft_sensor + + + + /icubSim/left_leg/analog:o + /baseestimation/l_leg_ft_sensor + + + + /icubSim/right_leg/analog:o + /baseestimation/r_leg_ft_sensor + + + + /icubSim/left_foot/analog:o + /baseestimation/l_foot_ft_sensor:i + + + + /icubSim/right_foot/analog:o + /baseestimation/r_foot_ft_sensor:i + + + + + + true + true + + 0.2 + + + + + diff --git a/devices/baseEstimatorV1/app/robots/iCubGenova04/fbe-analogsens.xml b/devices/baseEstimatorV1/app/robots/iCubGenova04/fbe-analogsens.xml new file mode 100644 index 0000000..e1fb908 --- /dev/null +++ b/devices/baseEstimatorV1/app/robots/iCubGenova04/fbe-analogsens.xml @@ -0,0 +1,97 @@ + + + + + + model.urdf + icub + 0.010 + ("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") + root_link + l_foot_ft_sensor + r_foot_ft_sensor + + false + false + true + 3.0 + + + + l_sole + l_sole + (0 0 0 0 0 0) + + + qekf + (1.0 0.0 0.0 0.0) + 0.0 + 0.0 + + + 0.010 + 0.7 + 0.001 + false + root_link_ems_acc_eb5 + (0 0 0) + + + 0.010 + 0.03 + 0.0 + 0.5 + 10e-11 + 10e-6 + 10e-1 + 10e-11 + 10e-3 + false + + + right + 0.5 + 0.5 + 130.0 + 25.0 + 150.0 + 25.0 + + + /wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o + /wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o + + + /logger/data:o + /logger/rpc:o + /logger-est/data:i + /logger-est/rpc:i + + + + + all_joints_mc + + + inertial + + left_upper_arm_strain + right_upper_arm_strain + left_upper_leg_strain + right_upper_leg_strain + left_lower_leg_strain + right_lower_leg_strain + + + + + + + + diff --git a/devices/baseEstimatorV1/app/robots/iCubGenova04/launch-fbe-analogsens.xml b/devices/baseEstimatorV1/app/robots/iCubGenova04/launch-fbe-analogsens.xml new file mode 100644 index 0000000..007f251 --- /dev/null +++ b/devices/baseEstimatorV1/app/robots/iCubGenova04/launch-fbe-analogsens.xml @@ -0,0 +1,93 @@ + + + + + + + /icub/torso + /baseestimation/torso + + + /icub/left_arm + /baseestimation/left_arm + + + /icub/right_arm + /baseestimation/right_arm + + + /icub/left_leg + /baseestimation/left_leg + + + /icub/right_leg + /baseestimation/right_leg + + + /icub/head + /baseestimation/head + + + + + + ("/icub/head", "/icub/torso", "/icub/left_arm", "/icub/right_arm", "/icub/left_leg", "/icub/right_leg") + ("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") + /baseestimation + + + + + /icub/xsens_inertial + /baseestimation/waist/imu:i + + + + + /icub/left_arm/analog:o + /baseestimation/l_arm_ft_sensor + + + + /icub/right_arm/analog:o + /baseestimation/r_arm_ft_sensor + + + + /icub/left_leg/analog:o + /baseestimation/l_leg_ft_sensor + + + + /icub/right_leg/analog:o + /baseestimation/r_leg_ft_sensor + + + + /icub/left_foot/analog:o + /baseestimation/l_foot_ft_sensor:i + + + + /icub/right_foot/analog:o + /baseestimation/r_foot_ft_sensor:i + + + + + + true + true + + 0.2 + + + + + diff --git a/devices/baseEstimatorV1/baseEstimatorV1.ini b/devices/baseEstimatorV1/baseEstimatorV1.ini new file mode 100644 index 0000000..0b8448e --- /dev/null +++ b/devices/baseEstimatorV1/baseEstimatorV1.ini @@ -0,0 +1,4 @@ +[plugin baseEstimatorV1] +type device +name baseEstimatorV1 +library baseEstimatorV1 diff --git a/devices/baseEstimatorV1/include/Utils.hpp b/devices/baseEstimatorV1/include/Utils.hpp new file mode 100644 index 0000000..af328f7 --- /dev/null +++ b/devices/baseEstimatorV1/include/Utils.hpp @@ -0,0 +1,234 @@ +/** + * @file Utils.hpp + * @authors Giulio Romualdi + * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2018 + */ + +#ifndef WALKING_UTILS_HPP +#define WALKING_UTILS_HPP + +// std +#include +#include + +// YARP +#include +#include +#include +#include + +// iDynTree +#include +#include +#include +#include + +// eigen +#include + +typedef iDynTree::SparseMatrix iDynSparseMatrix; + +/** + * Helper for iDynTree library. + */ +namespace iDynTreeHelper +{ + /** + * Triplets namespace add some useful function to manage triplets + */ + namespace Triplets + { + /** + * Merge two set of triplets output = [output, input] + * @param input is the input set of triplets; + * @param output is the output set of triplets. + */ + void pushTriplets(const iDynTree::Triplets& input, iDynTree::Triplets& output); + + /** + * Merge two set of triplets. The output triplets object represents a matrix while + * the input triplets represent a sub matrix. + * @param startingRow is the row position of the sub matrix; + * @param startingColumn is the column position of the sub matrix; + * @param input is the input set of triplets; + * @param output is the output set of triplets. + */ + void pushTripletsAsSubMatrix(const unsigned& startingRow, const unsigned& startingColumn, + const iDynTree::Triplets& input, + iDynTree::Triplets& output); + + /** + * Get triplets from yarp value list. It is useful if you want to convert a yarp list of triplets + * into a iDynTree Triplets object. + * @todo extend to non squared matrix. + * @param input the yarp value list; + * @param matrixDimension the dimension of the matrix. It used to check if the triplets are valid + * (i.e. the row and the column of each triplet has to be compatible with the matrix dimension); + * @param output is the idyntree triplets object. + * @return true/false in case of success/failure + */ + bool getTripletsFromValues(const yarp::os::Value& input, + const int& matrixDimension, + iDynTree::Triplets& output); + } + + /** + * SparseMatrix namespace add some useful function to manage iDynTree sparse matrices + */ + namespace SparseMatrix + { + /** + * Convert an eigen sparse matrix into an iDynTree sparse matrix + * @param eigenSparseMatrix is a column Major eigen sparse matrix. + * @return an iDynTree column major sparse matrix. + */ + iDynSparseMatrix fromEigen(const Eigen::SparseMatrix& eigenSparseMatrix); + } + + namespace Rotation + { + /** + * Transform a 3x3 matrix into a skew-symmetric matrix. + * @param input is a 3x3 matrix; + * @return a 3x3 skew-symmetric matrix + */ + iDynTree::Matrix3x3 skewSymmetric(const iDynTree::Matrix3x3& input); + } + + /** + * Given 2 angles, it returns the shortest angular difference. + * The result would always be -pi <= result <= pi. + * + * This function is taken from ROS angles [api](http://docs.ros.org/lunar/api/angles/html/namespaceangles.html#a4436fe67ae0c9df020f6779101bbefab). + * @param fromRad is the starting angle expressed in radians; + * @param toRad is the final angle expressed in radians. + * @return the shortest angular distance + */ + double shortestAngularDistance(const double& fromRad, const double& toRad); +} + +/** + * Helper for YARP library. + */ +namespace YarpHelper +{ + /** + * Add a vector of string to a property of a given name. + * @param prop yarp property; + * @param key is the key; + * @param list is the vector of strings that will be added into the property. + * @return true/false in case of success/failure + */ + bool addVectorOfStringToProperty(yarp::os::Property& prop, const std::string& key, + const std::vector& list); + + /** + * Convert a yarp list into a vector of string + * @param input is the pointer of a yarp value; + * @param output is the vector of strings. + * @return true/false in case of success/failure + */ + bool yarpListToStringVector(yarp::os::Value*& input, std::vector& output); + + /** + * Extract a string from a searchable object. + * @param config is the searchable object; + * @param key the name to check for; + * @param string is the string. + * @return true/false in case of success/failure + */ + bool getStringFromSearchable(const yarp::os::Searchable& config, const std::string& key, + std::string& string); + + /** + * Extract a double from a searchable object. + * @param config is the searchable object; + * @param key the name to check for; + * @param number is the double. + * @return true/false in case of success/failure + */ + bool getNumberFromSearchable(const yarp::os::Searchable& config, const std::string& key, + double& number); + + + /** + * Extract a double from a searchable object. + * @param config is the searchable object; + * @param key the name to check for; + * @param number is the integer. + * @return true/false in case of success/failure + */ + bool getNumberFromSearchable(const yarp::os::Searchable& config, const std::string& key, + int& number); + + + /** + * Convert a yarp value into an iDynTree::VectorFixSize + * @param input yarp value; + * @param output iDynTree::VectorFixSize. + * @return true/false in case of success/failure. + */ + template + bool yarpListToiDynTreeVectorFixSize(const yarp::os::Value& input, iDynTree::VectorFixSize& output); + + /** + * Convert a yarp value into an iDynTree::VectorDynSize + * @param input yarp value; + * @param output iDynTree::VectorDynSize if the size of this vector is different from the size of the + * YARP list it will be resized. + * @return true/false in case of success/failure. + */ + bool yarpListToiDynTreeVectorDynSize(const yarp::os::Value& input, iDynTree::VectorDynSize& output); + + /** + * Merge two vectors. vector = [vector, t] + * @param vector the original vector. The new elements will be add at the end of this vector; + * @param t vector containing the elements that will be merged with the original vector. + */ + template + void mergeSigVector(yarp::sig::Vector& vector, const T& t); + + /** + * Variadic fuction used to merge several vectors. + * @param vector the original vector. The new elements will be add at the end of this vector; + * @param t vector containing the elements that will be merged with the original vector. + * @param args list containing all the vector that will be merged. + */ + template + void mergeSigVector(yarp::sig::Vector& vector, const T& t, const Args&... args); + + /** + * Send a variadic vector through a yarp buffered port + * @param port is a Yarp buffered port + * @param args list containing all the vector that will be send. + */ + template + void sendVariadicVector(yarp::os::BufferedPort& port, const Args&... args); + + /** + * Add strings to a bottle. + * @param bottle this bottle will be filled. + * @param strings list containing all the string. + */ + void populateBottleWithStrings(yarp::os::Bottle& bottle, const std::initializer_list& strings); +} + +/** + * Helper for std library + */ +namespace StdHelper +{ + /** + * Allow you to append vector to a deque. + * @param input input vector; + * @param output output deque; + * @param initPoint point where the vector will be append to the deque + */ + template + bool appendVectorToDeque(const std::vector& input, std::deque& output, const size_t& initPoint); +} +#include "Utils.tpp" + +#endif diff --git a/devices/baseEstimatorV1/include/Utils.tpp b/devices/baseEstimatorV1/include/Utils.tpp new file mode 100644 index 0000000..53058b2 --- /dev/null +++ b/devices/baseEstimatorV1/include/Utils.tpp @@ -0,0 +1,101 @@ +/** + * @file Utils.tpp + * @authors Giulio Romualdi + * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2018 + */ + +// std +#include + +// YARP +#include + +template +bool YarpHelper::yarpListToiDynTreeVectorFixSize(const yarp::os::Value& input, iDynTree::VectorFixSize& output) +{ + if (input.isNull()) + { + yError() << "[yarpListToiDynTreeVectorFixSize] Empty input value."; + return false; + } + if (!input.isList() || !input.asList()) + { + yError() << "[yarpListToiDynTreeVectorFixSize] Unable to read the input list."; + return false; + } + yarp::os::Bottle *inputPtr = input.asList(); + + if (inputPtr->size() != n) + { + yError() << "[yarpListToiDynTreeVectorFixSize] The dimension set in the configuration file is not " + << n; + return false; + } + + for (int i = 0; i < inputPtr->size(); i++) + { + if (!inputPtr->get(i).isDouble() && !inputPtr->get(i).isInt()) + { + yError() << "[yarpListToiDynTreeVectorFixSize] The input is expected to be a double"; + return false; + } + output(i) = inputPtr->get(i).asDouble(); + } + return true; +} + +template +void YarpHelper::mergeSigVector(yarp::sig::Vector& vector, const T& t) +{ + for(int i= 0; i +void YarpHelper::mergeSigVector(yarp::sig::Vector& vector, const T& t, const Args&... args) +{ + for(int i= 0; i +void YarpHelper::sendVariadicVector(yarp::os::BufferedPort& port, const Args&... args) +{ + yarp::sig::Vector& vector = port.prepare(); + vector.clear(); + + mergeSigVector(vector, args...); + + port.write(); +} + +template +bool StdHelper::appendVectorToDeque(const std::vector& input, std::deque& output, const size_t& initPoint) +{ + if(initPoint > output.size()) + { + std::cerr << "[appendVectorToDeque] The init point has to be less or equal to the size of the output deque." + << std::endl; + return false; + } + + // resize the deque + output.resize(input.size() + initPoint); + + // Advances the iterator it by initPoint positions + typename std::deque::iterator it = output.begin(); + std::advance(it, initPoint); + + // copy the vector into the deque from the initPoint position + std::copy(input.begin(), input.end(), it); + + return true; +} diff --git a/devices/baseEstimatorV1/include/WalkingLogger.hpp b/devices/baseEstimatorV1/include/WalkingLogger.hpp new file mode 100644 index 0000000..e5989d8 --- /dev/null +++ b/devices/baseEstimatorV1/include/WalkingLogger.hpp @@ -0,0 +1,55 @@ +/** + * @file WalkingLogger.hpp + * @authors Giulio Romualdi + * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2018 + */ + +#ifndef WALKING_LOGGER_HPP +#define WALKING_LOGGER_HPP + +// YARP +#include +#include +#include +#include + +class WalkingLogger +{ + yarp::os::BufferedPort m_dataPort; /**< Data logger port. */ + yarp::os::RpcClient m_rpcPort; /**< RPC data logger port. */ + +public: + + /** + * Configure + * @param config yarp searchable configuration variable; + * @param name is the name of the module. + * @return true/false in case of success/failure. + */ + bool configure(const yarp::os::Searchable& config, const std::string& name); + + /** + * Start record. + * @param strings head of the logger file + * @return true/false in case of success/failure. + */ + bool startRecord(const std::initializer_list& strings); + + /** + * Quit the logger. + */ + void quit(); + + /** + * Send data to the logger. + * @param args all the vector containing the data that will be sent. + */ + template + void sendData(const Args&... args); +}; + +#include "WalkingLogger.tpp" + +#endif diff --git a/devices/baseEstimatorV1/include/WalkingLogger.tpp b/devices/baseEstimatorV1/include/WalkingLogger.tpp new file mode 100644 index 0000000..585c6f5 --- /dev/null +++ b/devices/baseEstimatorV1/include/WalkingLogger.tpp @@ -0,0 +1,15 @@ +/** + * @file WalkingLogger.tpp + * @authors Giulio Romualdi + * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2018 + */ + +#include + +template +void WalkingLogger::sendData(const Args&... args) +{ + YarpHelper::sendVariadicVector(m_dataPort, args...); +} diff --git a/devices/baseEstimatorV1/include/baseEstimatorV1.h b/devices/baseEstimatorV1/include/baseEstimatorV1.h new file mode 100644 index 0000000..5254d13 --- /dev/null +++ b/devices/baseEstimatorV1/include/baseEstimatorV1.h @@ -0,0 +1,626 @@ +/* + * Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + */ + +#ifndef BASE_ESTIMATOR_V1_H +#define BASE_ESTIMATOR_V1_H + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +inline double deg2rad(const double angleInDeg) +{ + return angleInDeg*M_PI/180.0; +} + +inline double rad2deg(const double angleInRad) +{ + return angleInRad*180.0/M_PI; +} + +inline void convertVectorFromDegreesToRadians(iDynTree::VectorDynSize & vector) +{ + for(size_t i=0; i < vector.size(); i++) + { + vector(i) = deg2rad(vector(i)); + } + + return; +} + +namespace yarp { + namespace dev { + + + class baseEstimatorV1 : public yarp::dev::DeviceDriver, + public yarp::dev::IMultipleWrapper, + public yarp::os::PeriodicThread, + public floatingBaseEstimationRPC + { + public: + explicit baseEstimatorV1(double period, yarp::os::ShouldUseSystemClock useSystemClock = yarp::os::ShouldUseSystemClock::No); + baseEstimatorV1(); + ~baseEstimatorV1(); + + /** + * @brief Open the estimator device + * @param[in] config Searchable object of configuration parameters + * @return true/false success/failure + */ + virtual bool open(yarp::os::Searchable& config); + + /** + * @brief Close the estimator device + * @return true/false success/failure + */ + virtual bool close(); + + /** + * @brief Attach other required devices to the estimator. Calls other attach methods. + * @param[in] p List of devices to be attached to the estimator + * @return true/false success/failure + */ + virtual bool attachAll(const yarp::dev::PolyDriverList& p); + + /** + * @brief detach other devices from the estimator device + * + * @return bool + */ + virtual bool detachAll(); + + /** + * @brief the periodic method that is called every update period, + * core of the estimator + * + * @return void + */ + virtual void run(); + + private: + /** + * @brief Load estimator device settings from configuration file + * + * @param[in] config parsed configuration file as a Searchable object + * @return bool + */ + bool loadEstimatorParametersFromConfig(const yarp::os::Searchable& config); + + /** + * @brief Load legged odometry settings from configuration file + * + * @param[in] config parsed configuration file as a Searchable object + * @return bool + */ + bool loadLeggedOdometryParametersFromConfig(const yarp::os::Searchable& config); + + /** + * @brief Load foot contact classifier settings from configuration file + * + * @param[in] config parsed configuration file as a Searchable object + * @return bool + */ + bool loadBipedFootContactClassifierParametersFromConfig(const yarp::os::Searchable& config); + + /** + * @brief Load attitude estimator (Mahony-Hamel filter) settings from configuration file + * + * @param[in] config parsed configuration file as a Searchable object + * @return bool + */ + bool loadIMUAttitudeMahonyEstimatorParametersFromConfig(const yarp::os::Searchable& config); + + /** + * @brief Load attitude estimator (Quaternion EKF) settings from configuration file + * + * @param[in] config parsed configuration file as a Searchable object + * @return bool + */ + bool loadIMUAttitudeQEKFParamtersFromConfig(const yarp::os::Searchable& config); + + /** + * @brief Instantiates the transform broadcaster to publish the world to base transform + * over a ROS topic for RViz visualization + * + * @return bool + */ + bool loadTransformBroadcaster(); + + /** + * @brief function taking in a pointer of readFunc() + * to perform a dry run initial check of sensors + * + * @param[in] verbose verbose argument to be passed to function pointer + * @param[in] func_t pointer of readFunc() + * @return true/false success/failure + */ + bool sensorReadDryRun(bool verbose, bool (baseEstimatorV1::*func_t)(bool)); + + /** + * @brief read IMU sensors + * + * @param[in] verbose verbose flag + * @return true/false success/failure + */ + bool readIMUSensors(bool verbose); + + /** + * @brief read FT sensors + * + * @param[in] verbose verbose flag + * @return true/false success/failure + */ + bool readFTSensors(bool verbose); + + /** + * @brief read Encoders + * + * @param[in] verbose verbose flag + * @return true/false success/failure + */ + bool readEncoders(bool verbose); + + /** + * @brief parent function calling all read sensors methods + * + * @param[in] verbose verbose flag + * @return true/false success/failure + */ + bool readSensors(bool verbose); + + /** + * @brief Loads SimpleLeggedOdometry, BipedFootContactClassifier and AttitudeMahonyFilter + * also KinDynComputations if necessary + * @return true/false success/failure + */ + bool loadEstimator(); + + /** + * @brief instantiate the legged odometry with loaded config parameters + * + * @return true/false success/failure + */ + bool loadLeggedOdometry(); + + /** + * @brief instantiate the biped foot contact classifier with loaded config parameters + * + * @return true/false success/failure + */ + bool loadBipedFootContactClassifier(); + + /** + * @brief instantiate the Mahony-Hamel filter with loaded config parameters + * + * @return true/false success/failure + */ + bool loadIMUAttitudeMahonyEstimator(); + + /** + * @brief instantiate the Quaternion EKF with loaded config parameters + * + * @return true/false success/failure + */ + bool loadIMUAttitudeQEKF(); + + /** + * @brief Attach all the control board units to the estimator + * @param[in] p List of devices to be attached to the estimator + * @return true/false success/failure + */ + bool attachAllControlBoards(const yarp::dev::PolyDriverList &p); + + /** + * @brief Attach all the inertial measurement units to the estimator + * @param[in] p List of devices to be attached to the estimator + * @return true/false success/failure + */ + bool attachAllInertialMeasurementUnits(const yarp::dev::PolyDriverList &p); + + /** + * @brief Attach all the 6 axis force-torque sensor units to the estimator + * @param[in] p List of devices to be attached to the estimator + * @return true/false success/failure + */ + bool attachAllForceTorqueSensors(const yarp::dev::PolyDriverList &p); + + /** + * @brief Attach the multiple analog sensor interface to the estimator + * @param[in] p List of devices to be attached to the estimator + * @return true/false success/failure + */ + bool attachMultipleAnalogSensors(const yarp::dev::PolyDriverList &p); + + /** + * @brief Allocating data buffers + */ + void resizeBuffers(); + + /** + * @brief Get the list of joints to be loaded by the estimator + * + * @param[in] config parsed configuration file as a Searchable object + * @param[out] joint_list list of joint names obtained from the configuration settings + * @return bool + */ + bool getJointNamesList(const yarp::os::Searchable& config, + std::vector& joint_list); + + /** + * @brief Open ports for publish and service calls + * @return true/false success/failure + */ + bool openComms(); + + /** + * @brief Close the estimator devicee + */ + void closeDevice(); + + /** + * @brief initialize legged odometry + * @return true/false success/failure + */ + bool initializeLeggedOdometry(); + + /** + * @brief initialize biped foot contact classifier + * @return true/false success/failure + */ + bool initializeBipedFootContactClassifier(); + + /** + * @brief initialize Mahony-Hamel filter + * @return true/false success/failure + */ + bool initializeIMUAttitudeEstimator(); + + /** + * @brief initialize quaternion EKF + * @return true/false success/failure + */ + bool initializeIMUAttitudeQEKF(); + + /** + * @brief compute transform of IMU's world frame with estimator world frame + * @return true/false success/failure + */ + bool alignIMUFrames(); + + /** + * @brief Get world to base rotation from attitude estimation using IMU + * @return iDynTree::Rotation return w_R_b computed using IMU rotation estimates + */ + iDynTree::Rotation getBaseOrientationFromIMU(); + + /** + * @brief Get the home transform of the neck to IMU to be used for correcting the + * transform due to neck kinematics. this is useful only when using the head IMU + * @note this method needs to be used only if using the head IMU, not applicable for the waist IMU + * @return iDynTree::Transform return HEADIMU_H_NeckBaseAtHomePosition + */ + iDynTree::Transform getHeadIMU_H_NeckBaseAtZero(); + + /** + * @brief Get the correction transform to be used for correcting the + * transform due to neck kinematics. this is useful only when using the head IMU + * @note this method needs to be used only if using the head IMU, not applicable for the waist IMU + * @return iDynTree::Transform return HEADIMU_H_NeckBase + */ + iDynTree::Transform getHeadIMUCorrectionWithNeckKinematics(); + + /** + * @brief update the fixed frame and kinematic measurements for legged odometry and + * update the world to base transform computed through legged odometery + * @return true/false success/failure + */ + bool updateLeggedOdometry(); + + /** + * @brief propagate the internal state of the Mahony-Hamel filter + * update the Mahony-Hamel filter with IMU measurements and + * @return true/false success/failure + */ + bool updateIMUAttitudeEstimator(); + + /** + * @brief propagate the internal state of the QEKF + * update the QEKF with IMU measurements and + * @return true/false success/failure + */ + bool updateIMUAttitudeQEKF(); + + /** + * @brief correct the orientation estimate of the Head IMU obtained from the + * attitude estimator with the correction transform HEADIMU_H_NeckBase + * @note this method needs to be used only if using the head IMU, not applicable for the waist IMU + * @return true/false success/failure + */ + bool correctHeadIMUWithNeckKinematics(); + + /** + * @brief Fuses the world to base transform + * obtained from legged odometry and IMU attitude estimation + * @return true/false success/failure + */ + bool updateBasePoseWithIMUEstimates(); + + /** + * @brief compute the floating base velocity using + * fixed frame contact Jacobian and holonomic constraint dynamics + * @return true/false success/failure + */ + bool updateBaseVelocity(); + + /** + * @brief computes the floating base velocity using IMU measurements + * linear velocity is obtained through Euler-integration of linear acceleration + * obtained from the proper sensor acceleration of the IMU + * angular velocity is given by the attitude estimator + * @return true/false success/failure + */ + bool updateBaseVelocityWithIMU(); + + /** + * @brief parent method for other publish methods + */ + void publish(); + + /** + * @brief publish internal state of attitude estimator through YARP port + * roll pitch yaw omegax omegay omegaz gyrobiasx gyrobiasy gyrobiasz + */ + void publishIMUAttitudeEstimatorStates(); + + /** + * @brief publish internal state of QEKF through YARP port + * roll pitch yaw + */ + void publishIMUAttitudeQEKFEstimates(); + + /** + * @brief publish floating base state through YARP port + * x y z roll pitch yaw joint_positions + * roll pitch yaw + */ + void publishFloatingBaseState(); + + /** + * @brief publish floating base pose and velocity through YARP port + * x y z roll pitch yaw vx vy vz omegax omegay omegaz + */ + void publishFloatingBasePoseVelocity(); + + /** + * @brief publish feet contact state through YARP port + * fz_lf fz_rf state_lf state_rf fixed_frame + */ + void publishContactState(); + + /** + * @brief publish world to base tranform to /tf ROS topic using IFrameTransform interface + */ + void publishTransform(); + + /** + * @brief configure whole body dynamics device and + * get the output ports to connect to in order + * to get the end-effector contact wrenches + * @return true/false success/failure + */ + bool configureWholeBodyDynamics(const yarp::os::Searchable& config); + + /** + * @brief calibrate the force-torque sensor offsets through a RPC call to wholebodydynamics + * this calibration is necessary for proper estimation of contact wrenches + * the calibration is internally taken care of by the wholebodydynamics device + * @return true/false success/failure + */ + bool calibFTSensorsStanding(); + + /** + * @brief get the foot contact normal forces + */ + void getFeetCartesianWrenches(); + + /** + * @brief read the foot contact wrenches from the output ports of the wholebodydynamics device + * @return true/false success/failure + */ + bool readWholeBodyDynamicsContactWrenches(bool verbose); + + /** + * @brief initialize logger with the necessary variables to be logged + * @return true/false success/failure + */ + bool initializeLogger(); + + /** + * @brief update the logger + * @return true/false success/failure + */ + bool updateLogger(); + + // RPC methods + virtual std::string getEstimationJointsList(); + virtual bool setMahonyKp(const double kp); + virtual bool setMahonyKi(const double ki); + virtual bool setMahonyTimeStep(const double timestep); + virtual bool setContactSchmittThreshold(const double l_fz_break, const double l_fz_make, + const double r_fz_break, const double r_fz_make); + virtual bool setPrimaryFoot(const std::string& primary_foot); + virtual std::string getRefFrameForWorld(); + virtual Pose6D getRefPose6DForWorld(); + virtual bool resetLeggedOdometry(); + virtual bool resetLeggedOdometryWithRefFrame(const std::string& ref_frame, + const double x, const double y, const double z, + const double roll, const double pitch, const double yaw); + virtual bool startFloatingBaseFilter(); + virtual bool useJointVelocityLPF(const bool flag); + virtual bool setJointVelocityLPFCutoffFrequency(const double freq); + + // device options + bool m_verbose{true}; ///< verbose outputs + std::string m_port_prefix{"/base-estimator"}; + + yarp::os::Mutex m_device_mutex; ///< mutex to avoid resource clash + + // status flags + bool m_device_initialized_correctly{false}; + bool m_legged_odometry_update_went_well{false}; + bool m_use_debug_ports{true}; + + // configuration parameters + enum class FilterFSM{IDLE, CONFIGURED, RUNNING}; + FilterFSM m_state{FilterFSM::IDLE}; + double m_device_period_in_s{0.01}; ///< Thread period of the estimator device in seconds + std::string m_model_file_name{"model.urdf"}; ///< URDF file name of the robot + std::string m_robot{"icubSim"}; ///< name of the robot to identify between real and sim + std::vector m_estimation_joint_names; ///< list of joints used for estimation + std::string m_base_link_name{"root_link"}; ///< floating base link + std::string m_initial_fixed_frame; ///< initial fixed frame for legged odometry + std::string m_initial_reference_frame_for_world; ///< frame in which initial world is expressed + iDynTree::Transform m_initial_reference_frame_H_world; ///< pose of the world w.r.t initial reference frame + iDynTree::SchmittParams m_left_foot_contact_schmitt_params, m_right_foot_contact_schmitt_params; ///< contact Schmitt trigger parameters for the feet + std::string m_initial_primary_foot{"left"}; ///< initial primary foot for the contact classifier + iDynTree::AttitudeMahonyFilterParameters m_imu_attitude_observer_params; ///< parameters for the attitude observer + iDynTree::AttitudeQuaternionEKFParameters m_imu_attitude_qekf_params; + std::string m_head_imu_name{"head_imu_acc_1x1"}; + + // robot model and sensors + iDynTree::Model m_model; ///< iDynTree object of loaded robot model + iDynTree::SensorsList m_sensors_list; ///< iDynTree object of loaded sensors list from URDF + + const double m_sensor_timeout_in_seconds{2.0}; ///< Timeout to check for sensor measurements during dry run initial check + const size_t m_nr_of_channels_in_YARP_IMU_sensor{12}; ///< Number of channels available in YARP IMU sensor output port + const size_t m_nr_of_channels_in_YARP_FT_sensor{6}; ///< Number of channels available in YARP FT sensor output port + bool m_use_multiple_analog_sensor_interface{false}; ///< Flag to switch between analog sensor interface or multiple analog sensor interface + + iDynTree::JointPosDoubleArray m_joint_positions; ///< joint positions array + iDynTree::VectorDynSize m_joint_velocities; ///< joint velocities array + + // struct maintaining the IMU measurement serialization + struct IMU_measurements + { + iDynTree::Vector3 linear_proper_acceleration; + iDynTree::Vector3 angular_velocity; + iDynTree::Vector3 angular_acceleration; + bool sensor_status{true}; + std::string sensor_name; + }; + + // struct for the remapped control board interfaces + struct + { + // cannot be a unique_ptr because it is just a reference to a resource owned by someone else + yarp::dev::IEncoders *encs{nullptr}; + }m_remapped_control_board_interfaces; + + iDynTree::SensorsMeasurements m_sensor_measurements; + + std::vector m_whole_body_imu_interface; ///< generic sensor interface for maintaining IMU sensors across the whole body + size_t m_nr_of_IMUs_detected{0}; ///< number of IMUs attached to the estimator device + std::vector m_raw_IMU_measurements; ///< a vector of IMU measurements associated to the vector of IMU sensors + + std::vector m_whole_body_forcetorque_interface; ///< analog sensor interface for maintaining FT seneor across whole body + size_t m_nr_of_forcetorque_sensors_detected{0}; ///< number of FT sensors attached to the estimator device + + // YARP Buffers + std::vector m_imu_meaaurements_from_yarp_server; ///< YARP buffer for IMU measuremnts coming from different IMU sensors + yarp::sig::Vector m_ft_measurements_from_yarp_server; ///< YARP buffer for FT measuremnts coming from different FT sensors + + // Estimation interfaces + std::unique_ptr m_legged_odometry; ///< legged odometry + std::unique_ptr m_biped_foot_contact_classifier; ///< foot contact classifier based on an internal contact force Schmitt Trigger + std::unique_ptr m_imu_attitude_observer; ///< attitude observer to estimate IMU orientation from IMU measurements + std::unique_ptr m_imu_attitude_qekf; + + std::string m_attitude_estimator_type{"qekf"}; + iDynTree::VectorDynSize m_initial_attitude_estimate_as_quaternion; + double m_imu_confidence_roll{0.5}; + double m_imu_confidence_pitch{0.5}; + + yarp::os::BufferedPort m_floating_base_state_port; ///< port to publish floating base pose plus joint positions + yarp::os::BufferedPort m_floating_base_pose_port; ///< port to publish floating base pose + yarp::os::BufferedPort m_contact_state_port; ///< port to publish (fzleft, fzright, left_contact, right_contact, fixedLinkIndex) + yarp::os::BufferedPort m_imu_attitude_observer_estimated_state_port; ///< port to publish rotation as RPY and IMU gyro bias + yarp::os::BufferedPort m_imu_attitude_qekf_estimated_state_port; ///< port to publish rotation as RPY and IMU gyro bias + yarp::os::Port m_estimator_rpc_port; ///< RPC port for service calls + + iDynTree::RPY m_imu_attitude_estimate_as_rpy; + + iDynTree::Rotation m_head_imu_calibration_matrix; + iDynTree::Transform m_imu_H_neck_base_at_zero; + bool m_imu_aligned{false}; + + std::string m_current_fixed_frame; ///< current frame associated to the fixed link in legged odometery + std::string m_previous_fixed_frame; ///< previous frame associated to the fixed link in legged odometery + bool m_no_foot_in_contact{false}; ///< flag to check if contact on both feet is lost + double m_left_foot_contact_normal_force, m_right_foot_contact_normal_force; ///< foot contact force z-direction + + yarp::sig::Vector m_world_pose_base_in_R6; ///< 6D vector pose of floating base frame in world reference frame + yarp::sig::Matrix m_world_H_base; ///< Homogeneous transformation matrix from base to world reference frame + yarp::sig::Vector m_world_velocity_base; ///< 6D vector velocity of floating base frame in the world reference frame + yarp::sig::Vector m_world_velocity_base_from_imu; + + std::string m_left_foot_ft_sensor{"l_foot_ft_sensor"}; + unsigned int m_left_foot_ft_sensor_index, m_right_foot_ft_sensor_index; + std::string m_right_foot_ft_sensor{"r_foot_ft_sensor"}; + std::string m_right_sole{"r_sole"}; + std::string m_left_sole{"l_sole"}; + iDynTree::Rotation m_l_sole_R_l_ft_sensor, m_r_sole_R_r_ft_sensor; + iDynTree::KinDynComputations m_kin_dyn_comp; + + std::string m_left_foot_cartesian_wrench_port_name, m_right_foot_cartesian_wrench_port_name; + yarp::os::BufferedPort m_left_foot_cartesian_wrench_wbd_port, m_right_foot_cartesian_wrench_wbd_port; + yarp::sig::Vector m_left_foot_cartesian_wrench, m_right_foot_cartesian_wrench; + bool m_wbd_is_open{false}; + + yarp::dev::PolyDriver m_transform_broadcaster; + yarp::dev::IFrameTransform *m_transform_interface{nullptr}; + + std::unique_ptr m_logger; + bool m_dump_data{false}; + + std::unique_ptr m_joint_velocities_filter; + double m_joint_vel_filter_cutoff_freq{0.0}; + double m_joint_vel_filter_sample_time_in_s{0.0}; + bool m_use_lpf{false}; + }; + } +} + +#endif diff --git a/devices/baseEstimatorV1/ros/fbeViz.launch b/devices/baseEstimatorV1/ros/fbeViz.launch new file mode 100644 index 0000000..6dfb57b --- /dev/null +++ b/devices/baseEstimatorV1/ros/fbeViz.launch @@ -0,0 +1,15 @@ + + + + + + + + + diff --git a/devices/baseEstimatorV1/scope/base_scope.xml b/devices/baseEstimatorV1/scope/base_scope.xml new file mode 100644 index 0000000..f4f73f9 --- /dev/null +++ b/devices/baseEstimatorV1/scope/base_scope.xml @@ -0,0 +1,212 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/devices/baseEstimatorV1/scope/base_velocity.xml b/devices/baseEstimatorV1/scope/base_velocity.xml new file mode 100644 index 0000000..4155617 --- /dev/null +++ b/devices/baseEstimatorV1/scope/base_velocity.xml @@ -0,0 +1,213 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/devices/baseEstimatorV1/scope/contact_scope.xml b/devices/baseEstimatorV1/scope/contact_scope.xml new file mode 100644 index 0000000..5ef2e14 --- /dev/null +++ b/devices/baseEstimatorV1/scope/contact_scope.xml @@ -0,0 +1,169 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/devices/baseEstimatorV1/scope/waist_imu_scope.xml b/devices/baseEstimatorV1/scope/waist_imu_scope.xml new file mode 100644 index 0000000..7362197 --- /dev/null +++ b/devices/baseEstimatorV1/scope/waist_imu_scope.xml @@ -0,0 +1,103 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/devices/baseEstimatorV1/src/Utils.cpp b/devices/baseEstimatorV1/src/Utils.cpp new file mode 100644 index 0000000..3b75ba0 --- /dev/null +++ b/devices/baseEstimatorV1/src/Utils.cpp @@ -0,0 +1,280 @@ +/** + * @file Utils.cpp + * @authors Giulio Romualdi + * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2018 + */ + +// std +#ifndef _USE_MATH_DEFINES +#define _USE_MATH_DEFINES +#endif +#include + +// YARP +#include + +// iDynTree +#include +#include + +iDynTree::Matrix3x3 iDynTreeHelper::Rotation::skewSymmetric(const iDynTree::Matrix3x3& input) +{ + iDynTree::Matrix3x3 output; + iDynTree::toEigen(output) = 0.5 * (iDynTree::toEigen(input) - iDynTree::toEigen(input).transpose()); + return output; +} + +void iDynTreeHelper::Triplets::pushTriplets(const iDynTree::Triplets& input, + iDynTree::Triplets& output) +{ + for(auto triplet: input) + output.pushTriplet(triplet); + + return; +} + +void iDynTreeHelper::Triplets::pushTripletsAsSubMatrix(const unsigned& startingRow, + const unsigned& startingColumn, + const iDynTree::Triplets& input, + iDynTree::Triplets& output) +{ + if(startingRow != 0 || startingColumn != 0) + { + iDynTree::Triplets inputShifted = input; + + for(auto triplet: inputShifted) + { + triplet.row += startingRow; + triplet.column += startingColumn; + output.pushTriplet(triplet); + } + } + else + { + iDynTreeHelper::Triplets::pushTriplets(input, output); + } + return; +} + +bool iDynTreeHelper::Triplets::getTripletsFromValues(const yarp::os::Value& input, + const int& matrixDimension, + iDynTree::Triplets& output) +{ + // clear the output + output.clear(); + + if (input.isNull()) + { + yError() << "[getTripletsFromValues] Empty input values."; + return false; + } + + if (!input.isList() || !input.asList()) + { + yError() << "[getSparseMatrixFromTriplets] Unable to read the input as a list."; + return false; + } + + yarp::os::Bottle *tripletsPtr = input.asList(); + + // populate the triplets + for (int i = 0; i < tripletsPtr->size(); ++i) + { + yarp::os::Bottle *tripletPtr = tripletsPtr->get(i).asList(); + + if (tripletPtr->size() != 3) + { + yError() << "[getSparseMatrixFromTriplets] The triplet must have three elements."; + return false; + } + + int row = tripletPtr->get(0).asInt(); + int col = tripletPtr->get(1).asInt(); + + if(col >= matrixDimension || row >= matrixDimension) + { + yError() << "[getSparseMatrixFromTriplets] element position exceeds the matrix dimension."; + return false; + } + output.pushTriplet(iDynTree::Triplet(col, row, tripletPtr->get(2).asDouble())); + } + return true; +} + +iDynSparseMatrix iDynTreeHelper::SparseMatrix::fromEigen(const Eigen::SparseMatrix& eigenSparseMatrix) +{ + iDynTree::Triplets triplets; + // populate the triplets + for (int k=0; k < eigenSparseMatrix.outerSize(); ++k) + for (Eigen::SparseMatrix::InnerIterator it(eigenSparseMatrix, k); it; ++it) + triplets.pushTriplet(iDynTree::Triplet(it.row(), it.col(), it.value())); + + // convert the triplets into a sparse matrix + iDynSparseMatrix iDynTreeSparseMatrix(eigenSparseMatrix.rows(), eigenSparseMatrix.cols()); + iDynTreeSparseMatrix.setFromConstTriplets(triplets); + + return iDynTreeSparseMatrix; +} + +bool YarpHelper::yarpListToiDynTreeVectorDynSize(const yarp::os::Value& input, iDynTree::VectorDynSize& output) +{ + if (input.isNull()) + { + yError() << "[yarpListToiDynTreeVectorDynSize] Empty input value."; + return false; + } + if (!input.isList() || !input.asList()) + { + yError() << "[yarpListToiDynTreeVectorDynSize] Unable to read the input list."; + return false; + } + yarp::os::Bottle *inputPtr = input.asList(); + + if (inputPtr->size() != output.size()) + { + yError() << "[yarpListToiDynTreeVectorDynSize] The size of the iDynTree vector and the size of " + << "the YARP list are not coherent."; + return false; + } + + for (int i = 0; i < inputPtr->size(); i++) + { + if (!inputPtr->get(i).isDouble() && !inputPtr->get(i).isInt()) + { + yError() << "[yarpListToiDynTreeVectorDynSize] The input is expected to be a double or a int"; + return false; + } + output(i) = inputPtr->get(i).asDouble(); + } + return true; +} + +bool YarpHelper::addVectorOfStringToProperty(yarp::os::Property& prop, const std::string& key, + const std::vector& list) +{ + // check if the key already exists + if(prop.check(key)) + { + yError() << "[addVectorOfStringToProperty] The property already exist."; + return false; + } + + prop.addGroup(key); + yarp::os::Bottle& bot = prop.findGroup(key).addList(); + for(size_t i=0; i < list.size(); i++) + bot.addString(list[i].c_str()); + + return true; +} + +bool YarpHelper::yarpListToStringVector(yarp::os::Value*& input, std::vector& output) +{ + // clear the std::vector + output.clear(); + + // check if the yarp value is a list + if(!input->isList()) + { + yError() << "[yarpListToStringVector] The input is not a list."; + return false; + } + + yarp::os::Bottle *bottle = input->asList(); + for(int i = 0; i < bottle->size(); i++) + { + // check if the elements of the bottle are strings + if(!bottle->get(i).isString()) + { + yError() << "[yarpListToStringVector] There is a field that is not a string."; + return false; + } + output.push_back(bottle->get(i).asString()); + } + return true; +} + +bool YarpHelper::getStringFromSearchable(const yarp::os::Searchable& config, const std::string& key, + std::string& string) +{ + yarp::os::Value* value; + if(!config.check(key, value)) + { + yError() << "[getStringFromSearchable] Missing field "<< key; + return false; + } + + if(!value->isString()) + { + yError() << "[getStringFromSearchable] the value is not a string."; + return false; + } + + string = value->asString(); + return true; +} + +bool YarpHelper::getNumberFromSearchable(const yarp::os::Searchable& config, const std::string& key, + double& number) +{ + yarp::os::Value* value; + if(!config.check(key, value)) + { + yError() << "[getNumberFromSearchable] Missing field "<< key; + return false; + } + + if(!value->isDouble()) + { + yError() << "[getNumberFromSearchable] the value is not a double."; + return false; + } + + number = value->asDouble(); + return true; +} + +bool YarpHelper::getNumberFromSearchable(const yarp::os::Searchable& config, const std::string& key, + int& number) +{ + yarp::os::Value* value; + if(!config.check(key, value)) + { + yError() << "[getNumberFromSearchable] Missing field "<< key; + return false; + } + + if(!value->isInt()) + { + yError() << "[getNumberFromSearchable] the value is not an integer."; + return false; + } + + number = value->asInt(); + return true; +} + +void YarpHelper::populateBottleWithStrings(yarp::os::Bottle& bottle, const std::initializer_list& strings) +{ + for(const auto& string : strings) + bottle.addString(string); +} + +double normalizeAnglePositive(const double& angle) +{ + return fmod(fmod(angle, 2.0 * M_PI) + 2.0 * M_PI, 2.0 * M_PI); +} + +double normalizeAngle(const double& angle) +{ + double a = normalizeAnglePositive(angle); + if (a > M_PI) + a -= 2.0 *M_PI; + return a; +} + +double iDynTreeHelper::shortestAngularDistance(const double& fromRad, const double& toRad) +{ + return normalizeAngle(toRad - fromRad); +} diff --git a/devices/baseEstimatorV1/src/WalkingLogger.cpp b/devices/baseEstimatorV1/src/WalkingLogger.cpp new file mode 100644 index 0000000..3848fa4 --- /dev/null +++ b/devices/baseEstimatorV1/src/WalkingLogger.cpp @@ -0,0 +1,91 @@ +/** + * @file WalkingLogger.cpp + * @authors Giulio Romualdi + * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2018 + */ + +// YARP +#include + +#include +#include + +bool WalkingLogger::configure(const yarp::os::Searchable& config, const std::string& name) +{ + std::string portInput, portOutput; + + // check if the config file is empty + if(config.isNull()) + { + yError() << "[configureLogger] Empty configuration for the force torque sensors."; + return false; + } + + // open the connect the data logger port + if(!YarpHelper::getStringFromSearchable(config, "dataLoggerOutputPort_name", portOutput)) + { + yError() << "[configureLogger] Unable to get the string from searchable."; + return false; + } + if(!YarpHelper::getStringFromSearchable(config, "dataLoggerInputPort_name", portInput)) + { + yError() << "[configureLogger] Unable to get the string from searchable."; + return false; + } + m_dataPort.open("/" + name + portOutput); + if(!yarp::os::Network::connect("/" + name + portOutput, portInput)) + { + yError() << "Unable to connect to port " << "/" + name + portOutput; + return false; + } + + // open the connect the rpc logger port + if(!YarpHelper::getStringFromSearchable(config, "dataLoggerRpcOutputPort_name", portOutput)) + { + yError() << "[configureLogger] Unable to get the string from searchable."; + return false; + } + if(!YarpHelper::getStringFromSearchable(config, "dataLoggerRpcInputPort_name", portInput)) + { + yError() << "[configureLogger] Unable to get the string from searchable."; + return false; + } + m_rpcPort.open("/" + name + portOutput); + if(!yarp::os::Network::connect("/" + name + portOutput, portInput)) + { + yError() << "Unable to connect to port " << "/" + name + portOutput; + return false; + } + return true; +} + +bool WalkingLogger::startRecord(const std::initializer_list& strings) +{ + yarp::os::Bottle cmd, outcome; + + YarpHelper::populateBottleWithStrings(cmd, strings); + + m_rpcPort.write(cmd, outcome); + if(outcome.get(0).asInt() != 1) + { + yError() << "[startWalking] Unable to store data"; + return false; + } + return true; +} + +void WalkingLogger::quit() +{ + // stop recording + yarp::os::Bottle cmd, outcome; + cmd.addString("quit"); + m_rpcPort.write(cmd, outcome); + if(outcome.get(0).asInt() != 1) + yInfo() << "[close] Unable to close the stream."; + + // close ports + m_dataPort.close(); + m_rpcPort.close(); +} diff --git a/devices/baseEstimatorV1/src/baseEstimatorV1.cpp b/devices/baseEstimatorV1/src/baseEstimatorV1.cpp new file mode 100644 index 0000000..12ca431 --- /dev/null +++ b/devices/baseEstimatorV1/src/baseEstimatorV1.cpp @@ -0,0 +1,991 @@ +/* + * Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + */ + +#include + + +bool yarp::dev::baseEstimatorV1::getJointNamesList(const yarp::os::Searchable& config, std::vector< std::string >& joint_list) +{ + yarp::os::Property property; + property.fromString(config.toString().c_str()); + + yarp::os::Bottle *property_joints_list = property.find("joints_list").asList(); + if (property_joints_list == 0) + { + yError() << "floatingBaseEstimatorV1: " << "Could not find \"joints_list\" parameter in configuration file."; + return false; + } + + joint_list.resize(property_joints_list->size()); + for (int joint_idx = 0; joint_idx < property_joints_list->size(); joint_idx++) + { + joint_list[joint_idx] = property_joints_list->get(joint_idx).asString().c_str(); + } + return true; +} + +void yarp::dev::baseEstimatorV1::resizeBuffers() +{ + m_joint_positions.resize(m_model); + m_joint_velocities.resize(m_joint_positions.size()); + m_joint_velocities.zero(); + + m_initial_attitude_estimate_as_quaternion.resize(4); + m_initial_attitude_estimate_as_quaternion.zero(); + m_initial_attitude_estimate_as_quaternion(0) = 1.0; + + m_world_pose_base_in_R6.resize(6); + m_world_velocity_base.resize(6); + m_world_velocity_base_from_imu.resize(6); + m_world_H_base.resize(4, 4); + m_left_foot_cartesian_wrench.resize(6); + m_right_foot_cartesian_wrench.resize(6); + + // wbd contact wrenches, ft sensors and imu measurement buffers are resized in the respective attach methods. +} + +yarp::dev::baseEstimatorV1::baseEstimatorV1(double period, yarp::os::ShouldUseSystemClock useSystemClock): PeriodicThread(period, useSystemClock) +{ +} + +yarp::dev::baseEstimatorV1::baseEstimatorV1(): PeriodicThread(0.01, yarp::os::ShouldUseSystemClock::No) +{ +} + + +bool yarp::dev::baseEstimatorV1::open(yarp::os::Searchable& config) +{ + yarp::os::LockGuard guard(m_device_mutex); + if (!configureWholeBodyDynamics(config)) + { + yError() << "floatingBaseEstimatorV1: " << "Could not connect to wholebodydynamics"; + return false; + } + + if (!loadEstimatorParametersFromConfig(config)) + { + yError() << "floatingBaseEstimatorV1: " << "Failed to load settings from configuration file"; + return false; + } + + if (!openComms()) + { + return false; + } + + if (m_dump_data) + { + m_logger = std::make_unique(); + yarp::os::Bottle& loggerOptions = config.findGroup("LOGGER"); + if (!m_logger->configure(loggerOptions, m_port_prefix)) + { + yError() << "[configure] Unable to configure the logger"; + return false; + } + } + + m_state = FilterFSM::IDLE; + return true; +} + +bool yarp::dev::baseEstimatorV1::loadLeggedOdometry() +{ + if (!m_model.setDefaultBaseLink(m_model.getFrameIndex(m_base_link_name))) + { + yWarning() << "floatingBaseEstimatorV1: " << "could not set default base link to " << m_base_link_name << + ". using link " << m_model.getDefaultBaseLink() << " instead."; + } + + m_legged_odometry = std::make_unique(); + if (!m_legged_odometry->setModel(m_model)) + { + yError() << "floatingBaseEstimatorV1: " << "failed to set model for legged odometry"; + return false; + } + + int axes{0}; + m_remapped_control_board_interfaces.encs->getAxes(&axes); + if (axes != (int)m_legged_odometry->model().getNrOfDOFs()) + { + yError() << "floatingBaseEstimatorV1: " << "estimator model and remapped control board interface has inconsistent number of joints"; + return false; + } + + return true; +} + +bool yarp::dev::baseEstimatorV1::loadBipedFootContactClassifier() +{ + m_biped_foot_contact_classifier = std::make_unique(m_left_foot_contact_schmitt_params, m_right_foot_contact_schmitt_params); + m_biped_foot_contact_classifier->setContactSwitchingPattern(iDynTree::ALTERNATE_CONTACT); + + return true; +} + +bool yarp::dev::baseEstimatorV1::loadIMUAttitudeMahonyEstimator() +{ + m_imu_attitude_observer = std::make_unique(); + m_imu_attitude_observer->setGainkp(m_imu_attitude_observer_params.kp); + m_imu_attitude_observer->setGainki(m_imu_attitude_observer_params.ki); + m_imu_attitude_observer->setTimeStepInSeconds(m_imu_attitude_observer_params.time_step_in_seconds); + m_imu_attitude_observer->useMagnetoMeterMeasurements(m_imu_attitude_observer_params.use_magnetometer_measurements); + return true; +} + +bool yarp::dev::baseEstimatorV1::loadIMUAttitudeQEKF() +{ + m_imu_attitude_qekf = std::make_unique(); + m_imu_attitude_qekf->setParameters(m_imu_attitude_qekf_params); + return true; +} + + +bool yarp::dev::baseEstimatorV1::initializeLeggedOdometry() +{ + bool ok = m_legged_odometry->updateKinematics(m_joint_positions); + ok = ok && m_legged_odometry->init(m_initial_fixed_frame, m_initial_reference_frame_for_world, m_initial_reference_frame_H_world); + yInfo() << "Base link set to: " << m_legged_odometry->model().getLinkName(m_legged_odometry->model().getDefaultBaseLink()); + yInfo() << "Initial world to base transform \n" << m_legged_odometry->getWorldLinkTransform(m_legged_odometry->model().getDefaultBaseLink()).toString(); + + return ok; +} + +bool yarp::dev::baseEstimatorV1::initializeBipedFootContactClassifier() +{ + if (m_initial_primary_foot == "left") + { + m_biped_foot_contact_classifier->setPrimaryFoot(iDynTree::BipedFootContactClassifier::LEFT_FOOT); + } + else if (m_initial_primary_foot == "right") + { + m_biped_foot_contact_classifier->setPrimaryFoot(iDynTree::BipedFootContactClassifier::RIGHT_FOOT); + } + else + { + yError() << "floatingBaseEstimatorV1: " << "initial primary foot not set."; + } + return true; +} + +bool yarp::dev::baseEstimatorV1::initializeIMUAttitudeEstimator() +{ + iDynTree::VectorDynSize state; + state.resize((int)m_imu_attitude_observer->getInternalStateSize()); + state.zero(); + + state(0) = m_initial_attitude_estimate_as_quaternion(0); + state(1) = m_initial_attitude_estimate_as_quaternion(1); + state(2) = m_initial_attitude_estimate_as_quaternion(2); + state(3) = m_initial_attitude_estimate_as_quaternion(3); + + iDynTree::Span stateBuf(state.data(), state.size()); + m_imu_attitude_observer->setInternalState(stateBuf); + return true; +} + +bool yarp::dev::baseEstimatorV1::initializeIMUAttitudeQEKF() +{ + if (!m_imu_attitude_qekf->initializeFilter()) + { + yInfo() << "floatingBaseEstimatorV1: " << "Could not initialize Qekf"; + return false; + } + + iDynTree::Vector10 state; + state.zero(); + + state(0) = m_initial_attitude_estimate_as_quaternion(0); + state(1) = m_initial_attitude_estimate_as_quaternion(1); + state(2) = m_initial_attitude_estimate_as_quaternion(2); + state(3) = m_initial_attitude_estimate_as_quaternion(3); + + iDynTree::Span state_span(state.data(), state.size()); + m_imu_attitude_qekf->setInternalState(state_span); + // Initial state covariance set during loadAttitudeQEKF + + return true; +} + + +void yarp::dev::baseEstimatorV1::getFeetCartesianWrenches() +{ + // get these wrenches from whole body dynamics to avoid errors due to calibration offsets + m_left_foot_contact_normal_force = m_left_foot_cartesian_wrench(2); + m_right_foot_contact_normal_force = m_right_foot_cartesian_wrench(2); +} + + +bool yarp::dev::baseEstimatorV1::updateLeggedOdometry() +{ + m_no_foot_in_contact = false; + m_biped_foot_contact_classifier->updateFootContactState(yarp::os::Time::now(), m_left_foot_contact_normal_force, m_right_foot_contact_normal_force); + switch (m_biped_foot_contact_classifier->getPrimaryFoot()) + { + case iDynTree::BipedFootContactClassifier::contactFoot::LEFT_FOOT : + { + m_current_fixed_frame = "l_sole"; + break; + } + case iDynTree::BipedFootContactClassifier::contactFoot::RIGHT_FOOT : + { + m_current_fixed_frame = "r_sole"; + break; + } + case iDynTree::BipedFootContactClassifier::contactFoot::UNKNOWN_FOOT : + { + m_current_fixed_frame = "unknown"; + m_no_foot_in_contact = true; + if (m_previous_fixed_frame != "unknown") + { + yError() << "floatingBaseEstimatorV1: " << "no foot in contact, failed to set fixed link"; + } + return false; + } + } + + if (m_previous_fixed_frame != m_current_fixed_frame) + { + m_legged_odometry_update_went_well = m_legged_odometry->updateKinematics(m_joint_positions); + iDynTree::Transform w_H_fixedFrame = m_legged_odometry->getWorldFrameTransform(m_legged_odometry->model().getFrameIndex(m_current_fixed_frame)); + iDynTree::Position w_H_fixedFrame_position = w_H_fixedFrame.getPosition(); + + // note flat terrain assumption + w_H_fixedFrame_position(2) = 0; + w_H_fixedFrame.setPosition(w_H_fixedFrame_position); + + if (m_legged_odometry->changeFixedFrame(m_current_fixed_frame, w_H_fixedFrame)) + { + yInfo() << "floatingBaseEstimatorV1: " << "legged odometry changed fixed frame to " << m_current_fixed_frame; + } + else + { + m_legged_odometry_update_went_well = false; + } + } + + m_legged_odometry_update_went_well = m_legged_odometry->updateKinematics(m_joint_positions); + + iDynTree::Transform w_H_b = m_legged_odometry->getWorldLinkTransform(m_legged_odometry->model().getDefaultBaseLink()); + //yInfo() << "World Pos: " << w_H_b.getPosition().toString(); + yarp::eigen::toEigen(m_world_pose_base_in_R6).block<3,1>(0,0) = iDynTree::toEigen(w_H_b.getPosition()); + yarp::eigen::toEigen(m_world_pose_base_in_R6).block<3,1>(3,0) = iDynTree::toEigen(w_H_b.getRotation().asRPY()); + iDynTree::toYarp(w_H_b.asHomogeneousTransform(), m_world_H_base); + + return m_legged_odometry_update_went_well; +} + +bool yarp::dev::baseEstimatorV1::updateIMUAttitudeEstimator() +{ + for (size_t imu = 0; imu < (size_t)m_whole_body_imu_interface.size(); imu++) + { + if (m_raw_IMU_measurements[imu].sensor_name == m_head_imu_name) + { + m_imu_attitude_observer->updateFilterWithMeasurements(m_raw_IMU_measurements[imu].linear_proper_acceleration, + m_raw_IMU_measurements[imu].angular_velocity); + + break; + } + } + + m_imu_attitude_observer->propagateStates(); + m_imu_attitude_observer->getOrientationEstimateAsRPY(m_imu_attitude_estimate_as_rpy); + return true; +} + +bool yarp::dev::baseEstimatorV1::updateIMUAttitudeQEKF() +{ + m_imu_attitude_qekf->propagateStates(); + for (size_t imu = 0; imu < (size_t)m_whole_body_imu_interface.size(); imu++) + { + if (m_raw_IMU_measurements[imu].sensor_name == m_head_imu_name) + { + m_imu_attitude_qekf->updateFilterWithMeasurements(m_raw_IMU_measurements[imu].linear_proper_acceleration, + m_raw_IMU_measurements[imu].angular_velocity); + + break; + } + } + return true; +} + +iDynTree::Transform yarp::dev::baseEstimatorV1::getHeadIMU_H_NeckBaseAtZero() +{ + iDynTree::KinDynComputations temp_kin_comp; + temp_kin_comp.loadRobotModel(m_model); + iDynTree::JointPosDoubleArray initial_positions{m_joint_positions}, initial_velocities{m_joint_positions}; + initial_velocities.zero(); + iDynTree::Vector3 gravity; + gravity.zero(); + gravity(2) = -9.81; + // set neck pitch, roll and yaw to zero + initial_positions(0) = initial_positions(1) = initial_positions(2) = 0.0; + temp_kin_comp.setRobotState(initial_positions, initial_velocities, gravity); + + return temp_kin_comp.getRelativeTransform(m_head_imu_name, "head"); +} + +iDynTree::Transform yarp::dev::baseEstimatorV1::getHeadIMUCorrectionWithNeckKinematics() +{ + // this funciton returns imu_H_imuAssumingNeckBaseToZero + if (!m_imu_aligned) + { + m_imu_H_neck_base_at_zero = getHeadIMU_H_NeckBaseAtZero(); + m_imu_aligned = true; + } + iDynTree::Transform imu_H_neck_base = m_kin_dyn_comp.getRelativeTransform(m_head_imu_name, "head"); + return imu_H_neck_base*(m_imu_H_neck_base_at_zero.inverse()); +} + + +bool yarp::dev::baseEstimatorV1::alignIMUFrames() +{ + iDynTree::Rotation b_R_head_imu = m_kin_dyn_comp.getRelativeTransform(m_base_link_name, m_head_imu_name).getRotation(); + iDynTree::Rotation wIMU_R_IMU_0; + if (m_attitude_estimator_type == "qekf") + { + m_imu_attitude_qekf->getOrientationEstimateAsRotationMatrix(wIMU_R_IMU_0); + } + else if (m_attitude_estimator_type == "mahony") + { + m_imu_attitude_observer->getOrientationEstimateAsRotationMatrix(wIMU_R_IMU_0); + } + else + { + yError() << "floatingBaseEstimatorV1: " << "Not using any attitude estimator, cannot align IMU frames"; + } + iDynTree::Rotation w_R_b = iDynTree::Rotation::RPY(m_world_pose_base_in_R6(3), m_world_pose_base_in_R6(4), m_world_pose_base_in_R6(5)); + m_head_imu_calibration_matrix = w_R_b*b_R_head_imu*wIMU_R_IMU_0.inverse(); + return true; +} + +iDynTree::Rotation yarp::dev::baseEstimatorV1::getBaseOrientationFromIMU() +{ + iDynTree::Rotation wIMU_R_IMU; + if (m_attitude_estimator_type == "mahony") + { + m_imu_attitude_observer->getOrientationEstimateAsRotationMatrix(wIMU_R_IMU); + } + else if (m_attitude_estimator_type == "qekf") + { + m_imu_attitude_qekf->getOrientationEstimateAsRotationMatrix(wIMU_R_IMU); + } + +// iDynTree::Rotation wIMU_R_wIMUNeckAtZero + // = getHeadIMUCorrectionWithNeckKinematics().getRotation(); + iDynTree::Rotation IMU_R_b = m_kin_dyn_comp.getRelativeTransform(m_head_imu_name, m_base_link_name).getRotation(); + + // correct IMU with neck kinematics +// iDynTree::Rotation wIMU_R_b = wIMU_R_wIMUNeckAtZero*wIMU_R_IMU*IMU_R_b; + iDynTree::Rotation wIMU_R_b = wIMU_R_IMU*IMU_R_b; + return (m_head_imu_calibration_matrix * wIMU_R_b); +} + +bool yarp::dev::baseEstimatorV1::updateBasePoseWithIMUEstimates() +{ + double updated_roll; + double updated_pitch; + iDynTree::Rotation w_R_b_imu = getBaseOrientationFromIMU(); + double weight_imu_on_roll{m_imu_confidence_roll}, weight_imu_on_pitch{m_imu_confidence_roll}; + std::string fixed_link; + + if (m_no_foot_in_contact) + { + weight_imu_on_pitch = 1.0; + weight_imu_on_roll = 1.0; + fixed_link = "r_foot"; + } + else + { + fixed_link = m_current_fixed_frame; + } + + // update base rotation + updated_roll = weight_imu_on_roll*w_R_b_imu.asRPY()(0) + (1 - weight_imu_on_roll) * m_world_pose_base_in_R6(3); + updated_pitch = weight_imu_on_pitch*w_R_b_imu.asRPY()(1) + (1 - weight_imu_on_pitch)* m_world_pose_base_in_R6(4); + + iDynTree::Rotation w_R_b = iDynTree::Rotation::RPY(updated_roll, updated_pitch, m_world_pose_base_in_R6(5)); + + // update base position + iDynTree::Transform w_H_b; + iDynTree::toiDynTree(m_world_H_base, w_H_b); + iDynTree::Transform b_H_fl = m_kin_dyn_comp.getRelativeTransform(m_base_link_name, fixed_link); + iDynTree::Position w_p_fl = (w_H_b*b_H_fl).getPosition(); + iDynTree::Position b_p_fl = b_H_fl.getPosition(); + iDynTree::Position w_p_b = w_p_fl - (w_R_b*b_p_fl); + + w_H_b.setRotation(w_R_b); + //w_H_b.setPosition(w_p_b); + + yarp::eigen::toEigen(m_world_pose_base_in_R6).block<3,1>(0,0) = iDynTree::toEigen(w_H_b.getPosition()); + yarp::eigen::toEigen(m_world_pose_base_in_R6).block<3,1>(3,0) = iDynTree::toEigen(w_H_b.getRotation().asRPY()); + iDynTree::toYarp(w_H_b.asHomogeneousTransform(), m_world_H_base); + + return true; +} + +bool yarp::dev::baseEstimatorV1::updateBaseVelocity() +{ + using iDynTree::toiDynTree; + using iDynTree::toEigen; + using iDynTree::toYarp; + + if (!m_model.getFrameIndex(m_initial_fixed_frame)) + { + return false; + } + iDynTree::Transform w_H_b_estimate; + toiDynTree(m_world_H_base, w_H_b_estimate); + + iDynTree::Vector3 gravity; + gravity(2) = -9.8; + + // we assume here the base velocity is zero, since the following computation + // gives us the floating jacobian which is dependent only on the joint positions + // and floating base pose due to mixed velocity representation + iDynTree::Twist base_velocity; + base_velocity.zero(); + m_kin_dyn_comp.setRobotState(w_H_b_estimate, m_joint_positions, base_velocity, m_joint_velocities, gravity); + + size_t n_joints = m_joint_velocities.size(); + iDynTree::MatrixDynSize contact_jacobian(6, (n_joints + 6)); + iDynTree::MatrixDynSize contact_jacobian_base(6, 6); + iDynTree::MatrixDynSize contact_jacobian_shape(6, n_joints); + + + if (!m_kin_dyn_comp.getFrameFreeFloatingJacobian(m_current_fixed_frame, contact_jacobian)) + { + yWarning() << "floatingBaseEstimatorV1: Could not compute the contact jacobian"; + return false; + } + + toEigen(contact_jacobian_base) = toEigen(contact_jacobian).block(0, 0, 6, 6); + toEigen(contact_jacobian_shape) = toEigen(contact_jacobian).block(0, 6, 6, n_joints); + + auto contact_jacobian_base_inverse{toEigen(contact_jacobian_base).inverse()}; + iDynTree::Vector6 floating_base_velocity; + toEigen(floating_base_velocity) = -contact_jacobian_base_inverse * toEigen(contact_jacobian_shape) * toEigen(m_joint_velocities); + toYarp(floating_base_velocity, m_world_velocity_base); + return true; +} + +bool yarp::dev::baseEstimatorV1::updateBaseVelocityWithIMU() +{ + using iDynTree::toEigen; + iDynTree::Vector3 y_acc; + // works only for waist IMU, waist IMU name is stored in the variable m_head_imu_name + for (size_t imu = 0; imu < (size_t)m_whole_body_imu_interface.size(); imu++) + { + if (m_raw_IMU_measurements[imu].sensor_name == m_head_imu_name) // change my name + { + y_acc = m_raw_IMU_measurements[imu].linear_proper_acceleration; + + break; + } + } + + // w_R_b*b_R_w*w_R_wimu(imu_ang_vel) + iDynTree::Rotation wIMU_R_IMU; + iDynTree::Vector10 attitude_estimator_state; + iDynTree::Span state_buffer(attitude_estimator_state.data(), attitude_estimator_state.size()); + if (m_attitude_estimator_type == "mahony") + { + m_imu_attitude_observer->getOrientationEstimateAsRotationMatrix(wIMU_R_IMU); + m_imu_attitude_observer->getInternalState(state_buffer); + } + else if (m_attitude_estimator_type == "qekf") + { + m_imu_attitude_qekf->getOrientationEstimateAsRotationMatrix(wIMU_R_IMU); + m_imu_attitude_qekf->getInternalState(state_buffer); + } + + iDynTree::Rotation w_R_IMU = m_head_imu_calibration_matrix*wIMU_R_IMU; + iDynTree::Vector3 gravity; + gravity.zero(); + gravity(2) = -9.8; + + iDynTree::Vector6 base_vel; + // compute left trivialized base velocity + auto imu_lin_vel{toEigen(base_vel).block<3, 1>(0, 0)}; + imu_lin_vel = imu_lin_vel + m_device_period_in_s*((toEigen(w_R_IMU)*toEigen(y_acc)) + toEigen(gravity)); + + iDynTree::Vector3 imu_ang_vel; + imu_ang_vel(0) = attitude_estimator_state(4); + imu_ang_vel(1) = attitude_estimator_state(5); + imu_ang_vel(2) = attitude_estimator_state(6); + toEigen(base_vel).block<3, 1>(3, 0) = (toEigen(w_R_IMU)*toEigen(imu_ang_vel)); + + iDynTree::toYarp(base_vel, m_world_velocity_base_from_imu); + return true; +} + + +void yarp::dev::baseEstimatorV1::publishFloatingBaseState() +{ + yarp::os::Bottle &state_bottle = m_floating_base_state_port.prepare(); + state_bottle.clear(); + for (unsigned int i = 0; i < m_world_pose_base_in_R6.size(); i++) + { + state_bottle.addDouble(m_world_pose_base_in_R6[i]); + } + + for (unsigned int i = 0; i < m_joint_positions.size(); i++) + { + state_bottle.addDouble(m_joint_positions(i)); + } + + m_floating_base_state_port.write(); +} + +void yarp::dev::baseEstimatorV1::publishFloatingBasePoseVelocity() +{ + yarp::os::Bottle &state_bottle = m_floating_base_pose_port.prepare(); + state_bottle.clear(); + for (unsigned int i = 0; i < m_world_pose_base_in_R6.size(); i++) + { + state_bottle.addDouble(m_world_pose_base_in_R6[i]); + } + + for (unsigned int i = 0; i < m_world_velocity_base.size(); i++) + { + state_bottle.addDouble(m_world_velocity_base[i]); + } + + m_floating_base_pose_port.write(); +} + +void yarp::dev::baseEstimatorV1::publishContactState() +{ + yarp::os::Bottle &state_bottle = m_contact_state_port.prepare(); + state_bottle.clear(); + state_bottle.addDouble(m_left_foot_contact_normal_force); + state_bottle.addDouble(m_right_foot_contact_normal_force); + state_bottle.addInt(m_biped_foot_contact_classifier->getLeftFootContactState()); + state_bottle.addInt(m_biped_foot_contact_classifier->getRightFootContactState()); + state_bottle.addString(m_current_fixed_frame); + m_contact_state_port.write(); +} + + +void yarp::dev::baseEstimatorV1::publishIMUAttitudeEstimatorStates() +{ + iDynTree::VectorDynSize attitude_observer_state; + if (m_attitude_estimator_type == "mahony") + { + attitude_observer_state.resize(m_imu_attitude_observer->getInternalStateSize()); + } + else if (m_attitude_estimator_type == "qekf") + { + attitude_observer_state.resize(m_imu_attitude_qekf->getInternalStateSize()); + } + + iDynTree::Span stateBuffer(attitude_observer_state.data(), attitude_observer_state.size()); + + if (m_attitude_estimator_type == "mahony") + { + m_imu_attitude_observer->getInternalState(stateBuffer); + } + else if (m_attitude_estimator_type == "qekf") + { + m_imu_attitude_qekf->getInternalState(stateBuffer); + } + + yarp::os::Bottle &state_bottle = m_imu_attitude_observer_estimated_state_port.prepare(); + state_bottle.clear(); + state_bottle.addDouble(rad2deg(m_imu_attitude_estimate_as_rpy(0))); // orientation roll + state_bottle.addDouble(rad2deg(m_imu_attitude_estimate_as_rpy(1))); // orientation pitch + state_bottle.addDouble(rad2deg(m_imu_attitude_estimate_as_rpy(2))); // orientation yaw + state_bottle.addDouble(rad2deg(attitude_observer_state(4))); // ang vel about x + state_bottle.addDouble(rad2deg(attitude_observer_state(5))); // ang vel about y + state_bottle.addDouble(rad2deg(attitude_observer_state(6))); // ang vel about z + state_bottle.addDouble(rad2deg(attitude_observer_state(7))); // gyro bias about x + state_bottle.addDouble(rad2deg(attitude_observer_state(8))); // gyro bias about y + state_bottle.addDouble(rad2deg(attitude_observer_state(9))); // gyro bias about z + + m_imu_attitude_observer_estimated_state_port.write(); +} + +void yarp::dev::baseEstimatorV1::publishIMUAttitudeQEKFEstimates() +{ + iDynTree::RPY rpy; + m_imu_attitude_qekf->getOrientationEstimateAsRPY(rpy); + + yarp::os::Bottle &state_bottle = m_imu_attitude_qekf_estimated_state_port.prepare(); + state_bottle.clear(); + state_bottle.addDouble(rad2deg(rpy(0))); + state_bottle.addDouble(rad2deg(rpy(1))); + state_bottle.addDouble(rad2deg(rpy(2))); + + m_imu_attitude_qekf_estimated_state_port.write(); +} + + +void yarp::dev::baseEstimatorV1::publishTransform() +{ + m_transform_interface->setTransform(m_base_link_name, "world", m_world_H_base); +} + +bool yarp::dev::baseEstimatorV1::initializeLogger() +{ + m_logger->startRecord({"record","fbe_x", "fbe_y", "fbe_z", + "fbe_roll", "fbe_pitch", "fbe_yaw", + "fbe_v_x", "fbe_v_y", "fbe_v_z", + "fbe_omega_x", "fbe_omega_y", "fbe_omega_z", + "fbe_lf_contact", "fbe_rf_contact", + "fbe_lf_fz", "fbe_rf_fz", + "attEst_roll", "attEst_pitch", "attEst_yaw", + "fbe_v_x_imu", "fbe_v_y_imu", "fbe_v_z_imu"}); + + return true; +} + +bool yarp::dev::baseEstimatorV1::updateLogger() +{ + yarp::sig::Vector feet_contact_state; + yarp::sig::Vector feet_contact_normal_forces; + feet_contact_state.resize(2); + feet_contact_normal_forces.resize(2); + feet_contact_state(0) = m_biped_foot_contact_classifier->getLeftFootContactState(); + feet_contact_state(1) = m_biped_foot_contact_classifier->getRightFootContactState(); + feet_contact_normal_forces(0) = m_left_foot_contact_normal_force; + feet_contact_normal_forces(1) = m_right_foot_contact_normal_force; + m_logger->sendData(m_world_pose_base_in_R6, m_world_velocity_base, feet_contact_state, + feet_contact_normal_forces, + m_imu_attitude_estimate_as_rpy, m_world_velocity_base_from_imu); + return true; +} + + +void yarp::dev::baseEstimatorV1::publish() +{ + publishFloatingBasePoseVelocity(); + publishContactState(); + + if (m_use_debug_ports) + { + publishFloatingBaseState(); + publishIMUAttitudeEstimatorStates(); + if (m_attitude_estimator_type == "qekf") + { + publishIMUAttitudeQEKFEstimates(); + } + } + publishTransform(); +} + +void yarp::dev::baseEstimatorV1::run() +{ + yarp::os::LockGuard guard(m_device_mutex); + + if(m_state != FilterFSM::RUNNING) + return; + + // if (!m_device_initialized_correctly) + // { + // calibFTSensorsStanding(); + // } + + if (readSensors(m_verbose)) + { + getFeetCartesianWrenches(); + + if (!m_device_initialized_correctly) + { + bool ok{false}; + ok = initializeLeggedOdometry(); + ok = initializeBipedFootContactClassifier(); + updateLeggedOdometry(); + if (m_attitude_estimator_type == "mahony") + { + ok = initializeIMUAttitudeEstimator(); + } + else if (m_attitude_estimator_type == "qekf") + { + ok = initializeIMUAttitudeQEKF(); + } + ok = alignIMUFrames(); + publish(); + if (m_dump_data) + { + initializeLogger(); + } + m_previous_fixed_frame = m_current_fixed_frame; + m_device_initialized_correctly = ok; + } + else + { + updateLeggedOdometry(); + if (m_attitude_estimator_type == "mahony") + { + updateIMUAttitudeEstimator(); + } + else if (m_attitude_estimator_type == "qekf") + { + updateIMUAttitudeQEKF(); + } + updateBasePoseWithIMUEstimates(); + updateBaseVelocity(); + //updateBaseVelocityWithIMU(); + publish(); + if (m_dump_data) + { + updateLogger(); + } + m_previous_fixed_frame = m_current_fixed_frame; + } + } + else + { + yError() << "floatingBaseEstimatorV1: " << " estimator will not return meaningful estimates, reading sensors failed"; + } + +} + +void yarp::dev::baseEstimatorV1::closeDevice() +{ + if (!m_imu_attitude_observer_estimated_state_port.isClosed()) + { + m_imu_attitude_observer_estimated_state_port.close(); + } + + if (!m_imu_attitude_qekf_estimated_state_port.isClosed()) + { + m_imu_attitude_qekf_estimated_state_port.close(); + } + + if (!m_floating_base_state_port.isClosed()) + { + m_floating_base_state_port.close(); + } + + if (!m_floating_base_pose_port.isClosed()) + { + m_floating_base_pose_port.close(); + } + + if (!m_contact_state_port.isClosed()) + { + m_contact_state_port.close(); + } + + if (m_estimator_rpc_port.isOpen()) + { + m_estimator_rpc_port.close(); + } + + m_device_initialized_correctly = false; + m_remapped_control_board_interfaces.encs = nullptr; + + if (m_transform_broadcaster.isValid()) + { + m_transform_broadcaster.close(); + } + + m_transform_interface = nullptr; + + if (m_wbd_is_open) + { + if (yarp::os::Network::isConnected(m_left_foot_cartesian_wrench_port_name, m_port_prefix + "/left_foot_cartesian_wrench:i")) + { + yarp::os::Network::disconnect(m_left_foot_cartesian_wrench_port_name, m_port_prefix + "/left_foot_cartesian_wrench:i"); + } + + if (yarp::os::Network::isConnected(m_right_foot_cartesian_wrench_port_name, m_port_prefix + "/right_foot_cartesian_wrench:i")) + { + yarp::os::Network::disconnect(m_right_foot_cartesian_wrench_port_name, m_port_prefix + "/right_foot_cartesian_wrench:i"); + } + } + if (!m_left_foot_cartesian_wrench_wbd_port.isClosed()) + { + m_left_foot_cartesian_wrench_wbd_port.close(); + } + + if (!m_right_foot_cartesian_wrench_wbd_port.isClosed()) + { + m_right_foot_cartesian_wrench_wbd_port.close(); + } + + if (m_dump_data) + { + m_logger->quit(); + } +} + +bool yarp::dev::baseEstimatorV1::close() +{ + yarp::os::LockGuard guard(m_device_mutex); + closeDevice(); + + return true; +} + +yarp::dev::baseEstimatorV1::~baseEstimatorV1() +{ + +} + +/// RPC methods + +bool yarp::dev::baseEstimatorV1::setMahonyKp(const double kp) +{ + yarp::os::LockGuard guard(m_device_mutex); + m_imu_attitude_observer->setGainkp(kp); + return true; +} + +bool yarp::dev::baseEstimatorV1::setMahonyKi(const double ki) +{ + yarp::os::LockGuard guard(m_device_mutex); + m_imu_attitude_observer->setGainki(ki); + return true; +} + +bool yarp::dev::baseEstimatorV1::setMahonyTimeStep(const double timestep) +{ + yarp::os::LockGuard guard(m_device_mutex); + m_imu_attitude_observer->setTimeStepInSeconds(timestep); + return true; +} + +bool yarp::dev::baseEstimatorV1::setContactSchmittThreshold(const double l_fz_break, const double l_fz_make, + const double r_fz_break, const double r_fz_make) +{ + yarp::os::LockGuard guard(m_device_mutex); + m_biped_foot_contact_classifier->m_leftFootContactClassifier->m_contactSchmitt->setLowValueThreshold(l_fz_break); + m_biped_foot_contact_classifier->m_leftFootContactClassifier->m_contactSchmitt->setHighValueThreshold(l_fz_make); + m_biped_foot_contact_classifier->m_rightFootContactClassifier->m_contactSchmitt->setLowValueThreshold(r_fz_break); + m_biped_foot_contact_classifier->m_rightFootContactClassifier->m_contactSchmitt->setHighValueThreshold(r_fz_make); + + m_biped_foot_contact_classifier->updateFootContactState(yarp::os::Time::now(), m_left_foot_contact_normal_force, + m_right_foot_contact_normal_force); + return true; +} + + +std::string yarp::dev::baseEstimatorV1::getEstimationJointsList() +{ + yarp::os::LockGuard guard(m_device_mutex); + std::stringstream ss; + for (int i = 0; i < m_estimation_joint_names.size(); i++) + { + if (i != m_estimation_joint_names.size() - 1) + { + ss << m_estimation_joint_names[i] << ","; + } + else + { + ss << m_estimation_joint_names[i]; + break; + } + } + + return ss.str(); +} + +bool yarp::dev::baseEstimatorV1::setPrimaryFoot(const std::string& primary_foot) +{ + yarp::os::LockGuard guard(m_device_mutex); + if (primary_foot == "right") + { + m_current_fixed_frame = "r_foot"; + m_biped_foot_contact_classifier->setPrimaryFoot(iDynTree::BipedFootContactClassifier::RIGHT_FOOT); + } + else if (primary_foot == "left") + { + m_current_fixed_frame = "l_foot"; + m_biped_foot_contact_classifier->setPrimaryFoot(iDynTree::BipedFootContactClassifier::LEFT_FOOT); + } + else + { + m_current_fixed_frame = "unknown"; + } + + m_previous_fixed_frame = "unknown"; + if (m_current_fixed_frame == "unknown") + { + return false; + } + m_legged_odometry->changeFixedFrame(m_current_fixed_frame); + return true; +} + +std::string yarp::dev::baseEstimatorV1::getRefFrameForWorld() +{ + yarp::os::LockGuard guard(m_device_mutex); + return m_initial_reference_frame_for_world; +} + +Pose6D yarp::dev::baseEstimatorV1::getRefPose6DForWorld() +{ + yarp::os::LockGuard guard(m_device_mutex); + Pose6D ref_pose_world; + ref_pose_world.x = m_initial_reference_frame_H_world.getPosition()(0); + ref_pose_world.y = m_initial_reference_frame_H_world.getPosition()(1); + ref_pose_world.z = m_initial_reference_frame_H_world.getPosition()(2); + ref_pose_world.roll = m_initial_reference_frame_H_world.getRotation().asRPY()(0); + ref_pose_world.pitch = m_initial_reference_frame_H_world.getRotation().asRPY()(1); + ref_pose_world.yaw = m_initial_reference_frame_H_world.getRotation().asRPY()(2); + return ref_pose_world; +} + +bool yarp::dev::baseEstimatorV1::resetLeggedOdometry() +{ + yarp::os::LockGuard guard(m_device_mutex); + m_legged_odometry->updateKinematics(m_joint_positions); + bool ok = m_legged_odometry->init(m_initial_fixed_frame, m_initial_reference_frame_for_world, m_initial_reference_frame_H_world); + return ok; +} + +bool yarp::dev::baseEstimatorV1::startFloatingBaseFilter() +{ + yarp::os::LockGuard guard(m_device_mutex); + m_state = FilterFSM::RUNNING; + return true; +} + +bool yarp::dev::baseEstimatorV1::resetLeggedOdometryWithRefFrame(const std::string& ref_frame, + const double x, const double y, const double z, + const double roll, const double pitch, const double yaw) +{ + yarp::os::LockGuard guard(m_device_mutex); + m_initial_reference_frame_for_world = ref_frame; + iDynTree::Position w_p_b(x, y ,z); + iDynTree::Rotation w_R_b{iDynTree::Rotation::RPY(roll, pitch, yaw)}; + + m_initial_reference_frame_H_world = iDynTree::Transform(w_R_b, w_p_b); + m_legged_odometry->updateKinematics(m_joint_positions); + m_initial_fixed_frame = ref_frame; + yInfo() << "Initial fixed frame changed to " << m_initial_fixed_frame; + bool ok = m_legged_odometry->init(m_initial_fixed_frame, m_initial_reference_frame_for_world, m_initial_reference_frame_H_world); + yInfo() << "Initial ref_frame to world transform: " << m_initial_reference_frame_H_world.toString(); + if (ref_frame == "r_sole") + { + m_biped_foot_contact_classifier->setPrimaryFoot(iDynTree::BipedFootContactClassifier::RIGHT_FOOT); + } + else if (ref_frame == "l_sole") + { + m_biped_foot_contact_classifier->setPrimaryFoot(iDynTree::BipedFootContactClassifier::LEFT_FOOT); + } + m_legged_odometry->changeFixedFrame(ref_frame); + + m_state = FilterFSM::RUNNING; + return ok; +} + +bool yarp::dev::baseEstimatorV1::setJointVelocityLPFCutoffFrequency(const double freq) +{ + m_joint_vel_filter_cutoff_freq = freq; + m_joint_velocities_filter->setCutFrequency(freq); + return true; +} + +bool yarp::dev::baseEstimatorV1::useJointVelocityLPF(const bool flag) +{ + m_use_lpf = flag; + return true; +} + diff --git a/devices/baseEstimatorV1/src/configureEstimator.cpp b/devices/baseEstimatorV1/src/configureEstimator.cpp new file mode 100644 index 0000000..696d6c8 --- /dev/null +++ b/devices/baseEstimatorV1/src/configureEstimator.cpp @@ -0,0 +1,499 @@ +/* + * Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + */ + +#include + +bool yarp::dev::baseEstimatorV1::loadEstimatorParametersFromConfig(const yarp::os::Searchable& config) +{ + if (config.check("model_file") && config.find("model_file").isString()) + { + m_model_file_name = config.find("model_file").asString(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Could not find \"model_file\" parameter in configuration file." << + " Loading default file with name " << m_model_file_name; + } + + if (config.check("robot") && config.find("robot").isString()) + { + m_robot = config.find("robot").asString(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Could not find \"robot\" parameter in configuration file." << + " Loading default robot name " << m_robot; + } + + if (config.check("device_period_in_seconds") && config.find("device_period_in_seconds").isDouble()) + { + m_device_period_in_s = config.find("device_period_in_seconds").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Could not find \"device_period_in_seconds\" parameter in configuration file." << + " Loading default device period " << m_device_period_in_s; + } + + if (!getJointNamesList(config, m_estimation_joint_names)) + { + return false; + } + + if (config.check("base_link") && config.find("base_link").isString()) + { + m_base_link_name = config.find("base_link").asString(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Could not find \"base_link\" parameter in configuration file." << + " Loading default robot name " << m_base_link_name; + } + + if (config.check("left_foot_ft_sensor") && config.find("left_foot_ft_sensor").isString()) + { + m_left_foot_ft_sensor = config.find("left_foot_ft_sensor").asString(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Could not find \"left_foot_ft_sensor\" parameter in configuration file." << + " Loading default FT sensor name " << m_left_foot_ft_sensor; + } + + if (config.check("right_foot_ft_sensor") && config.find("right_foot_ft_sensor").isString()) + { + m_right_foot_ft_sensor = config.find("right_foot_ft_sensor").asString(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Could not find \"right_foot_ft_sensor\" parameter in configuration file." << + " Loading default FT sensor name " << m_right_foot_ft_sensor; + } + + if (config.check("attitude_filter_type") && config.find("attitude_filter_type").isString()) + { + m_attitude_estimator_type = config.find("attitude_filter_type").asString(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Could not find \"attitude_filter_type\" parameter in configuration file." << + " Loading default attitude estimator type " << m_attitude_estimator_type; + } + + if (config.check("initial_attitude_estimate_as_quaternion") && config.find("initial_attitude_estimate_as_quaternion").isList()) + { + yarp::os::Bottle *attitude = config.find("initial_attitude_estimate_as_quaternion").asList(); + if (!attitude || attitude->size() != 4) + { + yWarning() << "floatingBaseEstimatorV1: " << "please mention \"initial_attitude_estimate_as_quaternion\" parameter as a list of 4 doubles."; + yWarning() << "floatingBaseEstimatorV1: " << "uing default value for initial attitude estimate: " << m_initial_reference_frame_for_world; + } + + m_initial_attitude_estimate_as_quaternion.resize(4); + + for (size_t i =0; i < 4; i++) + { + m_initial_attitude_estimate_as_quaternion(i) = attitude->get(i).asDouble(); + } + } + else + { + m_initial_attitude_estimate_as_quaternion.resize(4); + m_initial_attitude_estimate_as_quaternion.zero(); + m_initial_attitude_estimate_as_quaternion(0) = 1.0; + yWarning() << "floatingBaseEstimatorV1: " << "Could not find \"initial_attitude_estimate_as_quaternion\" parameter in configuration file." << + "uing default value for initial attitude estimate: " << m_initial_attitude_estimate_as_quaternion.toString(); + } + + if (config.check("imu_confidence_roll") && config.find("imu_confidence_roll").isDouble()) + { + m_imu_confidence_roll = config.find("imu_confidence_roll").asDouble(); + } + else + { + yError() << "floatingBaseEstimatorV1: " << "Could not find \"imu_confidence_roll\" parameter in configuration file." << + " using default value " << m_imu_confidence_roll ; + } + + if (config.check("imu_confidence_pitch") && config.find("imu_confidence_pitch").isDouble()) + { + m_imu_confidence_pitch = config.find("imu_confidence_pitch").asDouble(); + } + else + { + yError() << "floatingBaseEstimatorV1: " << "Could not find \"imu_confidence_pitch\" parameter in configuration file." << + " using default value " << m_imu_confidence_pitch ; + } + + if (config.check("publish_debug_ports") && config.find("publish_debug_ports").isBool()) + { + m_use_debug_ports = config.find("publish_debug_ports").asBool(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "debug mode set to set to " << m_use_debug_ports; + } + + bool ok = loadLeggedOdometryParametersFromConfig(config); + ok = loadBipedFootContactClassifierParametersFromConfig(config) && ok; + + if (config.check("imu_name") && config.find("imu_name").isString()) + { + m_head_imu_name = config.find("imu_name").asString(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will get IMU measurements from default IMU with name: " << m_head_imu_name ; + } + + if (m_attitude_estimator_type == "mahony") + { + ok = loadIMUAttitudeMahonyEstimatorParametersFromConfig(config) && ok; + } + else if (m_attitude_estimator_type == "qekf") + { + ok = loadIMUAttitudeQEKFParamtersFromConfig(config) && ok; + } + + m_dump_data = config.check("dump_data", yarp::os::Value(false)).asBool(); + + m_use_lpf = config.check("use_low_pass_filters", yarp::os::Value(false)).asBool(); + + if (m_use_lpf) + { + m_joint_vel_filter_cutoff_freq = config.check("joint_vel_lpf_cutoff_freq", yarp::os::Value(0.0)).asDouble(); + m_joint_vel_filter_sample_time_in_s = m_device_period_in_s; + } + + return ok; +} + +bool yarp::dev::baseEstimatorV1::loadLeggedOdometryParametersFromConfig(const yarp::os::Searchable& config) +{ + if (config.check("initial_fixed_frame") && config.find("initial_fixed_frame").isString()) + { + m_initial_fixed_frame = config.find("initial_fixed_frame").asString(); + } + else + { + yError() << "floatingBaseEstimatorV1: " << "Could not find \"initial_fixed_frame\" parameter in configuration file." << + " Exiting..." ; + return false; + } + + if (config.check("initial_reference_frame_for_world") && config.find("initial_reference_frame_for_world").isString()) + { + m_initial_reference_frame_for_world = config.find("initial_reference_frame_for_world").asString(); + } + else + { + m_initial_reference_frame_for_world = m_initial_fixed_frame; + yWarning() << "floatingBaseEstimatorV1: " << "Could not find \"initial_reference_frame_for_world\" parameter in configuration file." << + "Setting same value as initial fixed frame, " << m_initial_reference_frame_for_world; + m_initial_reference_frame_H_world.Identity(); + } + + if (config.check("initial_reference_frame_xyzrpy_pose_world") && config.find("initial_reference_frame_xyzrpy_pose_world").isList()) + { + yarp::os::Bottle *pose = config.find("initial_reference_frame_xyzrpy_pose_world").asList(); + if (!pose || pose->size() != 6) + { + yWarning() << "floatingBaseEstimatorV1: " << "please mention \"initial_reference_frame_xyzrpy_pose_world\" parameter as a list of 6 doubles."; + return false; + } + + iDynTree::Position initial_reference_frame_p_world; + initial_reference_frame_p_world(0) = pose->get(0).asDouble(); + initial_reference_frame_p_world(1) = pose->get(1).asDouble(); + initial_reference_frame_p_world(2) = pose->get(2).asDouble(); + + iDynTree::Rotation initial_reference_frame_R_world = iDynTree::Rotation::RPY(pose->get(3).asDouble(), + pose->get(4).asDouble(), + pose->get(5).asDouble()); + + m_initial_reference_frame_H_world = iDynTree::Transform(initial_reference_frame_R_world, initial_reference_frame_p_world); + } + else + { + m_initial_reference_frame_for_world = m_initial_fixed_frame; + yWarning() << "floatingBaseEstimatorV1: " << "Could not find \"initial_reference_frame_for_world\" parameter in configuration file." << + " Setting same value as initial fixed frame, " << m_initial_reference_frame_for_world; + m_initial_reference_frame_H_world.Identity(); + } + + return true; +} + +bool yarp::dev::baseEstimatorV1::loadBipedFootContactClassifierParametersFromConfig(const yarp::os::Searchable& config) +{ + if (config.check("initial_primary_foot") && config.find("initial_primary_foot").isString()) + { + m_initial_primary_foot = config.find("initial_primary_foot").asString(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Could not find \"initial_primary_foot\" parameter in configuration file." << + " Loading default initial primary foot " << m_initial_primary_foot; + } + + if (config.check("schmitt_stable_contact_make_time") && config.find("schmitt_stable_contact_make_time").isDouble()) + { + m_left_foot_contact_schmitt_params.stableTimeContactMake = config.find("schmitt_stable_contact_make_time").asDouble(); + m_right_foot_contact_schmitt_params.stableTimeContactMake = m_left_foot_contact_schmitt_params.stableTimeContactMake; + } + else + { + yError() << "floatingBaseEstimatorV1: " << "Could not find \"schmitt_stable_contact_make_time\" parameter in configuration file." << + " Exiting..." ; + return false; + } + + if (config.check("schmitt_stable_contact_break_time") && config.find("schmitt_stable_contact_break_time").isDouble()) + { + m_left_foot_contact_schmitt_params.stableTimeContactBreak = config.find("schmitt_stable_contact_break_time").asDouble(); + m_right_foot_contact_schmitt_params.stableTimeContactBreak = m_left_foot_contact_schmitt_params.stableTimeContactBreak; + } + else + { + yError() << "floatingBaseEstimatorV1: " << "Could not find \"schmitt_stable_contact_break_time\" parameter in configuration file." << + " Exiting..." ; + return false; + } + + if (config.check("left_schmitt_contact_make_force_threshold") && config.find("left_schmitt_contact_make_force_threshold").isDouble()) + { + m_left_foot_contact_schmitt_params.contactMakeForceThreshold = config.find("left_schmitt_contact_make_force_threshold").asDouble(); + } + else + { + yError() << "floatingBaseEstimatorV1: " << "Could not find \"left_schmitt_contact_make_force_threshold\" parameter in configuration file." << + " Exiting..." ; + return false; + } + + if (config.check("left_schmitt_contact_break_force_threshold") && config.find("left_schmitt_contact_break_force_threshold").isDouble()) + { + m_left_foot_contact_schmitt_params.contactBreakForceThreshold = config.find("left_schmitt_contact_break_force_threshold").asDouble(); + } + else + { + yError() << "floatingBaseEstimatorV1: " << "Could not find \"left_schmitt_contact_break_force_threshold\" parameter in configuration file." << + " Exiting..." ; + return false; + } + + if (config.check("right_schmitt_contact_make_force_threshold") && config.find("right_schmitt_contact_make_force_threshold").isDouble()) + { + m_right_foot_contact_schmitt_params.contactMakeForceThreshold = config.find("right_schmitt_contact_make_force_threshold").asDouble(); + } + else + { + yError() << "floatingBaseEstimatorV1: " << "Could not find \"right_schmitt_contact_make_force_threshold\" parameter in configuration file." << + " Exiting..." ; + return false; + } + + if (config.check("right_schmitt_contact_break_force_threshold") && config.find("right_schmitt_contact_break_force_threshold").isDouble()) + { + m_right_foot_contact_schmitt_params.contactBreakForceThreshold = config.find("right_schmitt_contact_break_force_threshold").asDouble(); + } + else + { + yError() << "floatingBaseEstimatorV1: " << "Could not find \"right_schmitt_contact_break_force_threshold\" parameter in configuration file." << + " Exiting..." ; + return false; + } + + return true; +} + +bool yarp::dev::baseEstimatorV1::loadIMUAttitudeMahonyEstimatorParametersFromConfig(const yarp::os::Searchable& config) +{ + if (config.check("mahony_kp") && config.find("mahony_kp").isDouble()) + { + m_imu_attitude_observer_params.kp = config.find("mahony_kp").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default gain kp: " << m_imu_attitude_observer_params.kp ; + } + + if (config.check("mahony_ki") && config.find("mahony_ki").isDouble()) + { + m_imu_attitude_observer_params.ki = config.find("mahony_ki").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default gain ki: " << m_imu_attitude_observer_params.ki ; + } + + if (config.check("mahony_use_magnetometer") && config.find("mahony_use_magnetometer").isBool()) + { + m_imu_attitude_observer_params.use_magnetometer_measurements = config.find("mahony_use_magnetometer").asBool(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "use magnetometer flag set to " << m_imu_attitude_observer_params.use_magnetometer_measurements; + } + + if (config.check("mahony_discretization_time_step_in_seconds") && config.find("mahony_discretization_time_step_in_seconds").isDouble()) + { + m_imu_attitude_observer_params.time_step_in_seconds = config.find("mahony_discretization_time_step_in_seconds").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default discretization time step: " << m_imu_attitude_observer_params.time_step_in_seconds ; + } + + return true; +} + +bool yarp::dev::baseEstimatorV1::loadIMUAttitudeQEKFParamtersFromConfig(const yarp::os::Searchable& config) +{ + if (config.check("qekf_discretization_time_step_in_seconds") && config.find("qekf_discretization_time_step_in_seconds").isDouble()) + { + m_imu_attitude_qekf_params.time_step_in_seconds = config.find("qekf_discretization_time_step_in_seconds").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default discretization time step: " << m_imu_attitude_qekf_params.time_step_in_seconds ; + } + + if (config.check("qekf_accelerometer_noise_variance") && config.find("qekf_accelerometer_noise_variance").isDouble()) + { + m_imu_attitude_qekf_params.accelerometer_noise_variance = config.find("qekf_accelerometer_noise_variance").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default accelerometer noise variance: " << m_imu_attitude_qekf_params.accelerometer_noise_variance ; + } + + if (config.check("qekf_magnetometer_noise_variance") && config.find("qekf_magnetometer_noise_variance").isDouble()) + { + m_imu_attitude_qekf_params.magnetometer_noise_variance = config.find("qekf_magnetometer_noise_variance").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default magnetometer noise variance: " << m_imu_attitude_qekf_params.magnetometer_noise_variance ; + } + + if (config.check("qekf_gyroscope_noise_variance") && config.find("qekf_gyroscope_noise_variance").isDouble()) + { + m_imu_attitude_qekf_params.gyroscope_noise_variance = config.find("qekf_gyroscope_noise_variance").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default gyroscope noise variance: " << m_imu_attitude_qekf_params.gyroscope_noise_variance ; + } + + if (config.check("qekf_gyro_bias_noise_variance") && config.find("qekf_gyro_bias_noise_variance").isDouble()) + { + m_imu_attitude_qekf_params.gyro_bias_noise_variance = config.find("qekf_gyro_bias_noise_variance").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default gyro bias noise variance: " << m_imu_attitude_qekf_params.gyro_bias_noise_variance ; + } + + if (config.check("qekf_initial_orientation_error_variance") && config.find("qekf_initial_orientation_error_variance").isDouble()) + { + m_imu_attitude_qekf_params.initial_orientation_error_variance = config.find("qekf_initial_orientation_error_variance").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default initial state orientation variance: " << m_imu_attitude_qekf_params.initial_orientation_error_variance ; + } + + if (config.check("qekf_initial_ang_vel_error_variance") && config.find("qekf_initial_ang_vel_error_variance").isDouble()) + { + m_imu_attitude_qekf_params.initial_ang_vel_error_variance = config.find("qekf_initial_ang_vel_error_variance").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default initial state angular velocity variance: " << m_imu_attitude_qekf_params.initial_ang_vel_error_variance ; + } + + if (config.check("qekf_initial_gyro_bias_error_variance") && config.find("qekf_initial_gyro_bias_error_variance").isDouble()) + { + m_imu_attitude_qekf_params.initial_gyro_bias_error_variance = config.find("qekf_initial_gyro_bias_error_variance").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default initial state gyro bias variance: " << m_imu_attitude_qekf_params.initial_gyro_bias_error_variance ; + } + + if (config.check("qekf_bias_correlation_time_factor") && config.find("qekf_bias_correlation_time_factor").isDouble()) + { + m_imu_attitude_qekf_params.bias_correlation_time_factor = config.find("qekf_bias_correlation_time_factor").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default bias forgetting factor: " << m_imu_attitude_qekf_params.bias_correlation_time_factor ; + } + + if (config.check("qekf_use_magnetometer_measurements") && config.find("qekf_use_magnetometer_measurements").isBool()) + { + m_imu_attitude_qekf_params.use_magnetometer_measurements = config.find("qekf_use_magnetometer_measurements").asBool(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "qekf use magnetometer flag set to " << m_imu_attitude_qekf_params.use_magnetometer_measurements; + } + + return true; +} + +bool yarp::dev::baseEstimatorV1::openComms() +{ + bool ok{false}; + ok = m_imu_attitude_observer_estimated_state_port.open(m_port_prefix + "/mahony_state/state:o"); + if (!ok) + { + yError() << "floatingBaseEstimatorV1: " << "could not open port " << m_port_prefix + "/mahony_state/state:o"; + return false; + } + + ok = m_imu_attitude_qekf_estimated_state_port.open(m_port_prefix + "/qekf/state:o"); + if (!ok) + { + yError() << "floatingBaseEstimatorV1: " << "could not open port " << m_port_prefix + "/qekf/state:o"; + return false; + } + + ok = m_floating_base_state_port.open(m_port_prefix + "/floating_base/configuration:o"); + if (!ok) + { + yError() << "floatingBaseEstimatorV1: " << "could not open port " << m_port_prefix + "/floating_base/configuration:o"; + return false; + } + + ok = m_floating_base_pose_port.open(m_port_prefix + "/floating_base/state:o"); + if (!ok) + { + yError() << "floatingBaseEstimatorV1: " << "could not open port " << m_port_prefix + "/floating_base/state:o"; + return false; + } + + ok = m_contact_state_port.open(m_port_prefix + "/feet_contact/state:o"); + if (!ok) + { + yError() << "floatingBaseEstimatorV1: " << "could not open port " << m_port_prefix + "/feet_contact/state:o"; + return false; + } + + floatingBaseEstimationRPC::yarp().attachAsServer(m_estimator_rpc_port); + ok = m_estimator_rpc_port.open(m_port_prefix + "/rpc"); + if (!ok) + { + yError() << "floatingBaseEstimatorV1: " << "could not open port " << m_port_prefix + "rpc"; + return false; + } + + return true; +} + diff --git a/devices/baseEstimatorV1/src/fbeRobotInterface.cpp b/devices/baseEstimatorV1/src/fbeRobotInterface.cpp new file mode 100644 index 0000000..19a03f2 --- /dev/null +++ b/devices/baseEstimatorV1/src/fbeRobotInterface.cpp @@ -0,0 +1,554 @@ +/* + * Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + */ + +#include + + +bool yarp::dev::baseEstimatorV1::sensorReadDryRun(bool verbose, bool (yarp::dev::baseEstimatorV1::*func_t)(bool)) +{ + double tic{yarp::os::Time::now()}; + double time_elapsed_trying_to_read_sensors{0.0}; + bool read_success{false}; + + while ((time_elapsed_trying_to_read_sensors < m_sensor_timeout_in_seconds) && !read_success) + { + read_success = (this->*func_t)(verbose); + time_elapsed_trying_to_read_sensors = (yarp::os::Time::now() - tic); + } + + if (!read_success) + { + yError() << "floatingBaseEstimatorV1: " << "unable to read from sensor correctly for " << m_sensor_timeout_in_seconds << " seconds.. exiting."; + } + return read_success; +} + +bool yarp::dev::baseEstimatorV1::attachAll(const yarp::dev::PolyDriverList& p) +{ + yarp::os::LockGuard guard(m_device_mutex); + if (!attachAllControlBoards(p)) + { + yError() << "baseEstimatorV1: " << "Could not attach the control boards"; + return false; + } + + if (!loadEstimator()) + { + yError() << "floatingBaseEstimatorV1: " << "Could not load estimator"; + return false; + } + + if (m_use_multiple_analog_sensor_interface) + { + if (!attachMultipleAnalogSensors(p)) + { + yError() << "floatingBaseEstimatorV1: " << "Could not attach the multiple analog sensor interface"; + return false; + } + } + else + { + if (!attachAllInertialMeasurementUnits(p)) + { + yError() << "floatingBaseEstimatorV1: " << "Could not attach the inertial measurement units"; + return false; + } + + if (!attachAllForceTorqueSensors(p)) + { + yError() << "floatingBaseEstimatorV1: " << "Could not attach the force-torque sensors"; + return false; + } + } + + start(); + return true; +} + +bool yarp::dev::baseEstimatorV1::attachAllControlBoards(const yarp::dev::PolyDriverList& p) +{ + bool ok{false}; + for (size_t dev_idx = 0; dev_idx < (size_t)p.size(); dev_idx++) + { + ok = p[dev_idx]->poly->view(m_remapped_control_board_interfaces.encs); + if (ok) + { + break; + } + } + + if (!ok) + { + return false; + } + + return true; +} + +bool yarp::dev::baseEstimatorV1::loadEstimator() +{ + yarp::os::ResourceFinder &rf = yarp::os::ResourceFinder::getResourceFinderSingleton(); + std::string model_file_path = rf.findFileByName(m_model_file_name); + yInfo() << "floatingBaseEstimatorV1: " << "Loading model from " + model_file_path; + + iDynTree::ModelLoader model_loader; ///< iDynTree object to load robot model + bool ok = model_loader.loadReducedModelFromFile(model_file_path, m_estimation_joint_names); + if (!ok) + { + yError() << "floatingBaseEstimatorV1: " << "Could not load model from specified path."; + return false; + } + + m_model = model_loader.model(); + m_sensors_list = model_loader.sensors(); + m_sensor_measurements.resize(m_sensors_list); + + m_kin_dyn_comp.loadRobotModel(m_model); + + resizeBuffers(); + setPeriod(m_device_period_in_s); + + ok = m_sensors_list.getSensorIndex(iDynTree::SIX_AXIS_FORCE_TORQUE, m_left_foot_ft_sensor, m_left_foot_ft_sensor_index) && ok; + ok = m_sensors_list.getSensorIndex(iDynTree::SIX_AXIS_FORCE_TORQUE, m_right_foot_ft_sensor, m_right_foot_ft_sensor_index) && ok; + + m_r_sole_R_r_ft_sensor = m_kin_dyn_comp.getRelativeTransform(m_model.getFrameIndex(m_right_sole), m_right_foot_ft_sensor_index).getRotation(); + m_l_sole_R_l_ft_sensor = m_kin_dyn_comp.getRelativeTransform(m_model.getFrameIndex(m_left_sole), m_left_foot_ft_sensor_index).getRotation(); + + ok = loadLeggedOdometry(); + ok = loadBipedFootContactClassifier() && ok; + if (m_attitude_estimator_type == "mahony") + { + ok = loadIMUAttitudeMahonyEstimator() && ok; + } + else if (m_attitude_estimator_type == "qekf") + { + ok = loadIMUAttitudeQEKF() && ok; + } + ok = loadTransformBroadcaster() && ok; + + if (!ok) + { + return false; + } + + return true; +} + +bool yarp::dev::baseEstimatorV1::attachMultipleAnalogSensors(const yarp::dev::PolyDriverList& p) +{ + return true; +} + +bool yarp::dev::baseEstimatorV1::attachAllForceTorqueSensors(const yarp::dev::PolyDriverList& p) +{ + std::vector ft_sensor_list; + std::vector ft_sensor_name; + for (size_t dev_idx = 0; dev_idx < (size_t)p.size(); dev_idx++) + { + // check if an analog sensor has 6 channels implying it is a forcetorque sensor + yarp::dev::IAnalogSensor* p_forcetorque = 0; + if (p[dev_idx]->poly->view(p_forcetorque)) + { + if (p_forcetorque->getChannels() == (int)m_nr_of_channels_in_YARP_FT_sensor) + { + ft_sensor_list.push_back(p_forcetorque); + ft_sensor_name.push_back(p[dev_idx]->key); + } + } + } + + if (ft_sensor_list.size() != m_sensors_list.getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE)) + { + yError() << "floatingBaseEstimatorV1: " << "Obtained " << m_sensors_list.getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE) << "from the model, but trying to attach " << (int)ft_sensor_list.size() << " FT sensors in the attach list."; + return false; + } + + m_whole_body_forcetorque_interface.resize(ft_sensor_list.size()); + for (size_t iDyn_sensor_idx = 0; iDyn_sensor_idx < m_whole_body_forcetorque_interface.size(); iDyn_sensor_idx++) + { + std::string sensor_name = m_sensors_list.getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE, iDyn_sensor_idx)->getName(); + // search in sensors list for ft sensor with same name as attach list + int idx_of_device_with_same_name{-1}; + for (size_t dev_idx = 0; dev_idx < ft_sensor_list.size(); dev_idx++) + { + if (ft_sensor_name[dev_idx] == sensor_name) + { + idx_of_device_with_same_name = dev_idx; + break; + } + } + + if (idx_of_device_with_same_name == -1) + { + yError() << "floatingBaseEstimatorV1: " << "was expecting a FT sensor with name " << sensor_name; + return false; + } + m_whole_body_forcetorque_interface[iDyn_sensor_idx] = ft_sensor_list[idx_of_device_with_same_name]; + } + + m_nr_of_forcetorque_sensors_detected = m_whole_body_forcetorque_interface.size(); + // dry run of readFTSensors() to check if all FTs work and to initialize buffers + m_ft_measurements_from_yarp_server.resize(m_nr_of_channels_in_YARP_FT_sensor); + bool verbose{false}; + if (!sensorReadDryRun(verbose, &yarp::dev::baseEstimatorV1::readFTSensors)) + { + return false; + } + + return true; +} + +bool yarp::dev::baseEstimatorV1::attachAllInertialMeasurementUnits(const yarp::dev::PolyDriverList& p) +{ + std::vector imu_sensor_list; + std::vector imu_sensor_name; + for (size_t dev_idx = 0; dev_idx < (size_t)p.size(); dev_idx++) + { + // check if a generic sensor has 12 channels implying it is a IMU sensor + yarp::dev::IGenericSensor* p_IMU = 0; + if (p[dev_idx]->poly->view(p_IMU)) + { + int nr_of_channels{0}; + p_IMU->getChannels(&nr_of_channels); + if (nr_of_channels == (int)m_nr_of_channels_in_YARP_IMU_sensor) + { + imu_sensor_list.push_back(p_IMU); + imu_sensor_name.push_back(p[dev_idx]->key); + } + } + } + + m_whole_body_imu_interface = imu_sensor_list; + m_nr_of_IMUs_detected = m_whole_body_imu_interface.size(); + + if (m_nr_of_IMUs_detected == 0) + { + yError() << "floatingBaseEstimatorV1: " << "Expecting atleast one IMU."; + return false; + } + + + m_raw_IMU_measurements.resize(m_nr_of_IMUs_detected); + // check this so that we can get sensor transforms from idyntree + // note this is a reverse check (different from the check on FT sensors) + for (size_t imu = 0; imu < m_nr_of_IMUs_detected; imu++) + { + bool found_imu{false}; + for (size_t iDyn_sensor_idx = 0; iDyn_sensor_idx < m_sensors_list.getNrOfSensors(iDynTree::ACCELEROMETER); iDyn_sensor_idx++) + { + std::string imu_name = m_sensors_list.getSensor(iDynTree::ACCELEROMETER, iDyn_sensor_idx)->getName(); + if (imu_sensor_name[imu] == imu_name) + { + found_imu = true; + break; + } + } + + if (!found_imu) + { + yError() << "floatingBaseEstimatorV1: " << "was expecting IMU with name from " << imu_sensor_name[imu] << " iDyntree Model"; + return false; + } + m_raw_IMU_measurements[imu].sensor_name = imu_sensor_name[imu]; + } + + // dry run of readIMUSensors() to check if all IMUs work and to initialize buffers + m_imu_meaaurements_from_yarp_server.resize(m_whole_body_imu_interface.size()); + for (size_t imu; imu < (size_t)m_whole_body_imu_interface.size(); imu++) + { + m_imu_meaaurements_from_yarp_server[imu].resize(m_nr_of_channels_in_YARP_IMU_sensor); + } + bool verbose{false}; + if (!sensorReadDryRun(verbose, &yarp::dev::baseEstimatorV1::readIMUSensors)) + { + return false; + } + return true; +} + +bool yarp::dev::baseEstimatorV1::readIMUSensors(bool verbose) +{ + bool all_IMUs_read_correctly{true}; + for (size_t imu = 0; imu < m_nr_of_IMUs_detected; imu++) + { + // TODO: get sensor name and associated transformation matrix + bool ok{true}; + m_raw_IMU_measurements[imu].angular_acceleration.zero(); + m_raw_IMU_measurements[imu].angular_velocity.zero(); + m_raw_IMU_measurements[imu].linear_proper_acceleration.zero(); + + ok = m_whole_body_imu_interface[imu]->read(m_imu_meaaurements_from_yarp_server[imu]); + m_raw_IMU_measurements[imu].sensor_status = ok; + if (!ok && verbose) + { + yWarning() << "floatingBaseEstimatorV1: " << "unable to read from IMU sensor " << m_raw_IMU_measurements[imu].sensor_name << " correctly. using old measurements."; + } + + if (ok) + { + m_raw_IMU_measurements[imu].angular_velocity(0) = deg2rad(m_imu_meaaurements_from_yarp_server[imu][6]); + m_raw_IMU_measurements[imu].angular_velocity(1) = deg2rad(m_imu_meaaurements_from_yarp_server[imu][7]); + m_raw_IMU_measurements[imu].angular_velocity(2) = deg2rad(m_imu_meaaurements_from_yarp_server[imu][8]); + + m_raw_IMU_measurements[imu].linear_proper_acceleration(0) = m_imu_meaaurements_from_yarp_server[imu][3]; + m_raw_IMU_measurements[imu].linear_proper_acceleration(1) = m_imu_meaaurements_from_yarp_server[imu][4]; + m_raw_IMU_measurements[imu].linear_proper_acceleration(2) = m_imu_meaaurements_from_yarp_server[imu][5]; + } + + all_IMUs_read_correctly = all_IMUs_read_correctly && ok; + } + + return all_IMUs_read_correctly; +} + +bool yarp::dev::baseEstimatorV1::readFTSensors(bool verbose) +{ + bool ft_sensors_read_correctly{true}; + for (size_t ft = 0; ft < m_nr_of_forcetorque_sensors_detected; ft++) + { + iDynTree::Wrench buffer_wrench; + int ft_ret_value = m_whole_body_forcetorque_interface[ft]->read(m_ft_measurements_from_yarp_server); + bool ok = (ft_ret_value == yarp::dev::IAnalogSensor::AS_OK); + ft_sensors_read_correctly = ft_sensors_read_correctly && ok; + + if (!ok && verbose) + { + yWarning() << "floatingBaseEstimatorV1: " << "unable to read from FT sensor " << m_sensors_list.getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE, ft)->getName() << " correctly. using old measurements."; + } + + bool is_NaN = false; + for (size_t i = 0; i < m_ft_measurements_from_yarp_server.size(); i++) + { + if (std::isnan(m_ft_measurements_from_yarp_server[i])) + { + is_NaN = true; + break; + } + } + + if (is_NaN) + { + yError() << "floatingBaseEstimatorV1: " << "FT sensor " << m_sensors_list.getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE, ft)->getName() << " contains nan: . using old measurements."<< m_ft_measurements_from_yarp_server.toString(); + return false; + } + + if (ok) + { + iDynTree::toiDynTree(m_ft_measurements_from_yarp_server, buffer_wrench); + m_sensor_measurements.setMeasurement(iDynTree::SIX_AXIS_FORCE_TORQUE, ft, buffer_wrench); + } + } + return ft_sensors_read_correctly; +} + +bool yarp::dev::baseEstimatorV1::readEncoders(bool verbose) +{ + int ax; m_remapped_control_board_interfaces.encs->getAxes(&ax); + bool encoders_read_correctly = m_remapped_control_board_interfaces.encs->getEncoders(m_joint_positions.data()); + + encoders_read_correctly = m_remapped_control_board_interfaces.encs->getEncoderSpeeds(m_joint_velocities.data()) && encoders_read_correctly; + if (!encoders_read_correctly && verbose) + { + yWarning() << "floatingBaseEstimatorV1: " << "unable to read from encoders interface properly"; + } + if (m_use_lpf) + { + if (!m_device_initialized_correctly) + { + m_joint_velocities_filter = std::make_unique(m_joint_vel_filter_cutoff_freq, m_device_period_in_s); + m_joint_velocities_filter->setSampleTime(m_device_period_in_s); + m_joint_velocities_filter->setCutFrequency(m_joint_vel_filter_cutoff_freq); + yarp::sig::Vector init_vel; + init_vel.resize(m_joint_velocities.size()); + iDynTree::toYarp(m_joint_velocities, init_vel); + m_joint_velocities_filter->init(init_vel); + } + else + { + yarp::sig::Vector unfilt_vel(m_joint_velocities.size()); + iDynTree::toYarp(m_joint_velocities, unfilt_vel); + const yarp::sig::Vector& filtered_velocities = m_joint_velocities_filter->filt(unfilt_vel); + iDynTree::toiDynTree(filtered_velocities, m_joint_velocities); + } + } + convertVectorFromDegreesToRadians(m_joint_positions); + convertVectorFromDegreesToRadians(m_joint_velocities); + return encoders_read_correctly; +} + + +bool yarp::dev::baseEstimatorV1::configureWholeBodyDynamics(const yarp::os::Searchable& config) +{ + if (config.check("left_foot_cartesian_wrench_port") && config.find("left_foot_cartesian_wrench_port").isString()) + { + m_left_foot_cartesian_wrench_port_name = config.find("left_foot_cartesian_wrench_port").asString(); + } + else + { + yError() << "floatingBaseEstimatorV1: " << "Could not find \"left_foot_cartesian_wrench_port\" parameter in configuration file." << + " Exiting..." ; + return false; + } + + if (config.check("right_foot_cartesian_wrench_port") && config.find("right_foot_cartesian_wrench_port").isString()) + { + m_right_foot_cartesian_wrench_port_name = config.find("right_foot_cartesian_wrench_port").asString(); + } + else + { + yError() << "floatingBaseEstimatorV1: " << "Could not find \"right_foot_cartesian_wrench_port\" parameter in configuration file." << + " Exiting..." ; + return false; + } + + bool ok = m_left_foot_cartesian_wrench_wbd_port.open(m_port_prefix + "/left_foot_cartesian_wrench:i"); + ok = yarp::os::Network::connect(m_left_foot_cartesian_wrench_port_name, m_port_prefix + "/left_foot_cartesian_wrench:i") && ok; + if (!ok) + { + yError() << "floatingBaseEstimatorV1: " << " could not connect to " << m_left_foot_cartesian_wrench_port_name; + return false; + } + + ok = m_right_foot_cartesian_wrench_wbd_port.open(m_port_prefix + "/right_foot_cartesian_wrench:i"); + ok = yarp::os::Network::connect(m_right_foot_cartesian_wrench_port_name, m_port_prefix + "/right_foot_cartesian_wrench:i") && ok; + if (!ok) + { + yError() << "floatingBaseEstimatorV1: " << " could not connect to " << m_right_foot_cartesian_wrench_port_name; + return false; + } + + m_right_foot_cartesian_wrench.resize(6); + m_left_foot_cartesian_wrench.resize(6); + + bool verbose{false}; + if (!sensorReadDryRun(verbose, &yarp::dev::baseEstimatorV1::readWholeBodyDynamicsContactWrenches)) + { + return false; + } + + m_wbd_is_open = true; + return true; +} + +bool yarp::dev::baseEstimatorV1::calibFTSensorsStanding() +{ + yarp::os::RpcClient wbd_rpc_port; + wbd_rpc_port.open("/wholeBodyDynamics/local/rpc"); + yarp::os::Network::connect("/wholeBodyDynamics/local/rpc", "/wholeBodyDynamics/rpc"); + yarp::os::Bottle calib_command; + calib_command.addString("calibStanding"); + calib_command.addString("all"); + yarp::os::Bottle calib_response; + + // writing five times for sanity check + for (int i = 0; i < 5; i++) + { + yarp::os::Time::delay(0.01); + wbd_rpc_port.write(calib_command, calib_response); + } + + if (calib_response.toString() != "[ok]") + { + return false; + } + return true; +} + +bool readCartesianWrenchesFromPorts(yarp::os::BufferedPort& port, yarp::sig::Vector& wrench, bool verbose) +{ + yarp::sig::Vector* raw_wrench; + + raw_wrench = port.read(false); + if (raw_wrench != nullptr) + { + wrench = *raw_wrench; + if (wrench.size() !=6 && verbose) + { + yError() << "floatingBaseEstimatorV1: " << "wrench size mismatch in left foot port."; + } + + bool is_NaN = false; + for (size_t i = 0; i < wrench.size(); i++) + { + if (std::isnan(wrench[i])) + { + is_NaN = true; + break; + } + } + + if (is_NaN) + { + yError() << "floatingBaseEstimatorV1: " << "foot cartesian wrench contains nan: . using old measurements." << wrench.toString(); + return false; + } + } + return true; +} + +bool yarp::dev::baseEstimatorV1::readWholeBodyDynamicsContactWrenches(bool verbose) +{ + bool ok = readCartesianWrenchesFromPorts(m_left_foot_cartesian_wrench_wbd_port, m_left_foot_cartesian_wrench, m_verbose); + ok = readCartesianWrenchesFromPorts(m_right_foot_cartesian_wrench_wbd_port, m_right_foot_cartesian_wrench, m_verbose) && ok; + + return ok; +} + +bool yarp::dev::baseEstimatorV1::readSensors(bool verbose) +{ + bool all_sensors_read_correctly{true}; + all_sensors_read_correctly = readEncoders(m_verbose) && all_sensors_read_correctly; + all_sensors_read_correctly = readFTSensors(m_verbose) && all_sensors_read_correctly; + all_sensors_read_correctly = readIMUSensors(m_verbose) && all_sensors_read_correctly; + all_sensors_read_correctly = readWholeBodyDynamicsContactWrenches(m_verbose) && all_sensors_read_correctly; + + return all_sensors_read_correctly; +} + +bool yarp::dev::baseEstimatorV1::loadTransformBroadcaster() +{ + yarp::os::Property tf_broadcaster_settings; + tf_broadcaster_settings.put("device", "transformClient"); + tf_broadcaster_settings.put("remote", "/transformServer"); + tf_broadcaster_settings.put("local", m_port_prefix + "/transformClient"); + + tf_broadcaster_settings.addGroup("axesNames"); + yarp::os::Bottle& axes_bottle = tf_broadcaster_settings.findGroup("axesNames").addList(); + + + for (size_t i = 0; i < m_estimation_joint_names.size(); i++) + { + axes_bottle.addString(m_estimation_joint_names[i]); + } + + if (!m_transform_broadcaster.open(tf_broadcaster_settings)) + { + yError() << "floatingBaseEstimatorV1: " << "could not open transform broadcaster."; + return false; + } + + if (!m_transform_broadcaster.view(m_transform_interface)) + { + yError() << "floatingBaseEstimatorV1: " << "could not access transform interface"; + return false; + } + + return true; +} + +bool yarp::dev::baseEstimatorV1::detachAll() +{ + yarp::os::LockGuard guard(m_device_mutex); + m_device_initialized_correctly = false; + if (isRunning()) + { + stop(); + } + return true; +} + diff --git a/devices/baseEstimatorV1/thrifts/floatingBaseEstimationRPC.thrift b/devices/baseEstimatorV1/thrifts/floatingBaseEstimationRPC.thrift new file mode 100644 index 0000000..f86be3b --- /dev/null +++ b/devices/baseEstimatorV1/thrifts/floatingBaseEstimationRPC.thrift @@ -0,0 +1,35 @@ +/* + * Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + */ + +struct Pose6D +{ + 1: double x; 2: double y; 3: double z; + 4: double roll; 5: double pitch; 6: double yaw; +} + +service floatingBaseEstimationRPC +{ + string getEstimationJointsList(); + bool setMahonyKp(1: double kp); + bool setMahonyKi(1: double ki); + bool setMahonyTimeStep(1: double timestep); + bool setContactSchmittThreshold(1: double l_fz_break, 2: double l_fz_make, + 3: double r_fz_break, 4: double r_fz_make); + + bool setPrimaryFoot(1: string primary_foot); + + bool resetLeggedOdometry(); + bool resetLeggedOdometryWithRefFrame(1: string ref_frame, 2: double x, 3: double y, 4: double z, + 5: double roll, 6: double pitch, 7: double yaw); + string getRefFrameForWorld(); + Pose6D getRefPose6DForWorld(); + bool useJointVelocityLPF(1: bool flag); + bool setJointVelocityLPFCutoffFrequency(1: double freq); + bool startFloatingBaseFilter(); +} + diff --git a/doc/resources/fbeV1.png b/doc/resources/fbeV1.png new file mode 100644 index 0000000..8bbf675 Binary files /dev/null and b/doc/resources/fbeV1.png differ