├── .github ├── CODEOWNERS └── ISSUE_TEMPLATE │ ├── feature_request.md │ └── bug_report.md ├── devices ├── Paexo │ ├── conf │ │ ├── paexo-cmake-config.h.in │ │ └── paexo-validation-motor-control.sh │ ├── test-application │ │ ├── CMakeLists.txt │ │ └── paexo-test-application.cpp │ └── CMakeLists.txt ├── IWearRemapper │ ├── conf │ │ └── IWearRemapper.ini │ └── CMakeLists.txt ├── FTShoes │ ├── src │ │ └── FTShoes.cpp │ ├── CMakeLists.txt │ └── include │ │ └── FTShoes.h ├── HumanWrenchProvider │ ├── WrenchFrameTransformers.cpp │ ├── WrenchFrameTransformers.h │ ├── CMakeLists.txt │ └── HumanWrenchProvider.h ├── CMakeLists.txt ├── XsensSuit │ ├── CMakeLists.txt │ └── conf │ │ └── XsensSuit.xml ├── ICub │ ├── CMakeLists.txt │ └── conf │ │ └── ICub.xml ├── IAnalogSensorToIWear │ ├── CMakeLists.txt │ └── conf │ │ └── ianalogsensor_to_iwear.xml ├── HumanControlBoard │ └── CMakeLists.txt ├── IFrameTransformToIWear │ ├── CMakeLists.txt │ └── conf │ │ └── iframetransform_to_iwear.xml ├── RobotPositionController │ ├── CMakeLists.txt │ └── RobotPositionController.h ├── HumanLogger │ ├── HumanLogger.h │ └── CMakeLists.txt ├── HumanDynamicsEstimator │ ├── CMakeLists.txt │ └── HumanDynamicsEstimator.h └── HumanStateProvider │ ├── CMakeLists.txt │ └── HumanStateProvider.h ├── misc ├── hde_scheme.png └── humanModel.png ├── impl ├── CMakeLists.txt └── SensorsImpl │ └── CMakeLists.txt ├── conf ├── ros │ ├── package.xml │ └── launch │ │ ├── iCubRviz.launch │ │ ├── iCub3Rviz.launch │ │ ├── twoHumansRviz.launch │ │ └── HDERviz.launch ├── xml │ ├── TransformServer.xml │ ├── applications │ │ ├── HumanDynamicsEstimation.xml │ │ ├── XsensRetargetingPositionControlPoleiCub3.xml │ │ ├── XsensRetargetingPositionControlPoleiCub2_5.xml │ │ ├── XsensRetargetingVisualizationiCub3.xml │ │ ├── XsensRetargetingVisualizationiCub2_5.xml │ │ ├── HumanDynamicsEstimation-HumanDynamics.xml │ │ ├── HumanDynamicsEstimation-HumanKinematics.xml │ │ └── HDERetargeting_iCub2_5_ifeel.xml │ ├── RobotPosePublisher.xml │ ├── RobotPositionController_iCub2_5.xml │ ├── HumanLoggerExample.xml │ ├── RobotPositionController_iCub.xml │ └── transformServer_to_iwear.xml ├── app │ ├── HumanStateVisualizer.ini │ ├── HumanStateVisualizer_iCub3.ini │ ├── HumanStateVisualizer_iCub2_5.ini │ ├── HumanStateVisualizerWithDynamics.ini │ ├── HumanStateVisualizer_iCub2_5_openxr.ini │ ├── HumanStateVisualizer_iRonCub_openxr.ini │ ├── HumanStateVisualizer_ergoCub_openxr.ini │ ├── HumanStateVisualizer_iRonCub_openxr_ifeel.ini │ ├── HumanStateVisualizer_iCub3_openxr.ini │ ├── HumanStateVisualizer_ergoCub_openxr_ifeel.ini │ ├── HumanStateVisualizer_iCub2_5_openxr_ifeel.ini │ └── HumanStateVisualizer_iCub3_openxr_ifeel.ini ├── HDE-yarpmanager.ini ├── iFeelOpenXRCalibration.sh └── CMakeLists.txt ├── HumanDynamicsEstimationLibrary ├── CMakeLists.txt ├── utils │ └── CMakeLists.txt └── algorithms │ └── CMakeLists.txt ├── clients ├── CMakeLists.txt ├── HumanWrench_nwc_yarp │ ├── conf │ │ ├── HumanWrenchRemapperExample.xml │ │ └── HumanWrench_nwc_yarpExample.xml │ ├── CMakeLists.txt │ └── HumanWrench_nwc_yarp.h ├── HumanDynamics_nwc_yarp │ ├── conf │ │ └── HumanDynamics_nwc_yarpExample.xml │ ├── CMakeLists.txt │ └── HumanDynamics_nwc_yarp.h ├── HumanState_nwc_yarp │ ├── CMakeLists.txt │ └── HumanState_nwc_yarp.h └── WearableTargets_nwc_yarp │ ├── CMakeLists.txt │ └── WearableTargets_nwc_yarp.h ├── servers ├── CMakeLists.txt ├── HumanState_nws_yarp │ ├── CMakeLists.txt │ └── HumanState_nws_yarp.h ├── HumanWrench_nws_yarp │ ├── CMakeLists.txt │ └── HumanWrench_nws_yarp.h ├── HumanDynamics_nws_yarp │ ├── CMakeLists.txt │ └── HumanDynamics_nws_yarp.h └── WearableTargets_nws_yarp │ ├── CMakeLists.txt │ └── WearableTargets_nws_yarp.h ├── .gitignore ├── doc ├── README.md ├── How-to-run-FTshoes.md └── How-to-run-iCub-as-wearable-source.md ├── modules ├── CMakeLists.txt ├── IWearFrameVisualizer │ └── CMakeLists.txt └── HumanStateVisualizer │ └── CMakeLists.txt ├── wrappers ├── CMakeLists.txt ├── IWear │ ├── CMakeLists.txt │ └── include │ │ └── IWearWrapper.h ├── IWearActuators │ ├── CMakeLists.txt │ └── include │ │ └── IWearActuatorsWrapper.h ├── IWearLogger │ ├── CMakeLists.txt │ └── include │ │ └── IWearLogger.h └── IXsensMVNControl │ ├── CMakeLists.txt │ └── include │ └── IXsensMVNControlWrapper.h ├── interfaces ├── CMakeLists.txt ├── IXsensMVNControl │ ├── CMakeLists.txt │ └── IXsensMVNControl.h ├── IHumanState │ ├── CMakeLists.txt │ └── hde │ │ └── interfaces │ │ └── IHumanState.h ├── IHumanWrench │ ├── CMakeLists.txt │ └── hde │ │ └── interfaces │ │ └── IHumanWrench.h ├── IHumanDynamics │ ├── CMakeLists.txt │ └── hde │ │ └── interfaces │ │ └── IHumanDynamics.h ├── IWear │ ├── include │ │ └── Wearable │ │ │ └── IWear │ │ │ ├── Common.h │ │ │ ├── Actuators │ │ │ ├── IHeater.h │ │ │ ├── IMotor.h │ │ │ ├── IHaptic.h │ │ │ └── IActuator.h │ │ │ └── Sensors │ │ │ ├── IGyroscope.h │ │ │ ├── IMagnetometer.h │ │ │ ├── IForce3DSensor.h │ │ │ ├── ISkinSensor.h │ │ │ ├── IAccelerometer.h │ │ │ ├── IPositionSensor.h │ │ │ ├── ITorque3DSensor.h │ │ │ ├── IEmgSensor.h │ │ │ ├── ITemperatureSensor.h │ │ │ ├── IFreeBodyAccelerationSensor.h │ │ │ ├── IVirtualJointKinSensor.h │ │ │ ├── IVirtualSphericalJointKinSensor.h │ │ │ ├── IOrientationSensor.h │ │ │ ├── IForceTorque6DSensor.h │ │ │ └── IPoseSensor.h │ └── CMakeLists.txt └── IWearableTargets │ └── CMakeLists.txt ├── ci_env.yml ├── XSensMVN ├── test │ ├── CMakeLists.txt │ └── testCalibrator.cpp ├── include │ ├── XSensCalibrationQualities.h │ ├── XSensMVNDriverStates.h │ └── XSensLogger.h └── CMakeLists.txt ├── app ├── CMakeLists.txt ├── Wearables-yarpmanager.ini └── xml │ ├── applications │ ├── HumanDynamicsEstimation-WearableDumper.xml │ ├── WearableDevices-Dumper.xml │ └── WearableDevices.xml │ ├── ICubWearableDevice.xml │ └── iWearLoggerExampleTemplate.xml ├── msgs ├── thrift │ └── XsensSuitControlService.thrift └── CMakeLists.txt ├── cmake ├── AddHDEPythonModule.cmake ├── AddWearablesPythonModule.cmake ├── FindSenseGlove.cmake ├── AddWarningsConfigurationToTarget.cmake └── FindXsensXME.cmake └── LICENSE /.github/CODEOWNERS: -------------------------------------------------------------------------------- 1 | * @traversaro @S-Dafarra @Gianlucamilani @davidegorbani 2 | -------------------------------------------------------------------------------- /devices/Paexo/conf/paexo-cmake-config.h.in: -------------------------------------------------------------------------------- 1 | #cmakedefine ENABLE_PAEXO_USE_iFEELDriver 2 | -------------------------------------------------------------------------------- /misc/hde_scheme.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotology/human-dynamics-estimation/HEAD/misc/hde_scheme.png -------------------------------------------------------------------------------- /misc/humanModel.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotology/human-dynamics-estimation/HEAD/misc/humanModel.png -------------------------------------------------------------------------------- /devices/IWearRemapper/conf/IWearRemapper.ini: -------------------------------------------------------------------------------- 1 | iwearPorts ("/iwear/ftShoes:o", "/iwear/xsens:o") 2 | portPrefix /iwearRemapper 3 | -------------------------------------------------------------------------------- /impl/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | 5 | add_subdirectory(SensorsImpl) 6 | -------------------------------------------------------------------------------- /conf/ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | HDERviz 3 | 4 | This is a simple package.xml file for configuring HDE rviz files. 5 | 6 | 7 | -------------------------------------------------------------------------------- /HumanDynamicsEstimationLibrary/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | # Include AddComponent cmake module 5 | include(AddComponent) 6 | 7 | add_subdirectory(algorithms) 8 | add_subdirectory(utils) 9 | -------------------------------------------------------------------------------- /clients/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | add_subdirectory(HumanState_nwc_yarp) 5 | add_subdirectory(HumanDynamics_nwc_yarp) 6 | add_subdirectory(HumanWrench_nwc_yarp) 7 | add_subdirectory(WearableTargets_nwc_yarp) 8 | -------------------------------------------------------------------------------- /servers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | add_subdirectory(HumanState_nws_yarp) 5 | add_subdirectory(HumanDynamics_nws_yarp) 6 | add_subdirectory(HumanWrench_nws_yarp) 7 | add_subdirectory(WearableTargets_nws_yarp) 8 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.DS_Store 2 | build* 3 | *kdev4 4 | *~ 5 | *.txt.user* 6 | *.vscode* 7 | .vs/ 8 | build/ 9 | build*/ 10 | install/ 11 | CMakeLists.txt.* 12 | .DS_Store 13 | *.bak 14 | *.orig 15 | *.autosave 16 | *.original 17 | *.*~ 18 | .vscode/c_cpp_properties.json 19 | .vscode/settings.json 20 | .vs/* 21 | CMakeSettings.json 22 | -------------------------------------------------------------------------------- /doc/README.md: -------------------------------------------------------------------------------- 1 | ## Documentation 2 | 3 | Guidelines on how to run and use wearable sensor source devices. 4 | 5 | ### Available documentation 6 | 7 | - [How-to-run-FTshoes](How-to-run-FTshoes.md) 8 | 9 | - [How-to-run-XsensSuit](How-to-run-XsensSuit.md) 10 | 11 | - [How-to-run-HapticGlove](How-to-compile-and-run-HapticGlove.md) 12 | -------------------------------------------------------------------------------- /modules/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | if(ENABLE_FrameVisualizer) 5 | add_subdirectory(IWearFrameVisualizer) 6 | endif() 7 | 8 | if(HUMANSTATEPROVIDER_ENABLE_VISUALIZER) 9 | add_subdirectory(HumanStateVisualizer) 10 | endif() 11 | -------------------------------------------------------------------------------- /wrappers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | 5 | add_subdirectory(IWear) 6 | add_subdirectory(IWearActuators) 7 | add_subdirectory(IXsensMVNControl) 8 | 9 | if(ENABLE_Logger) 10 | add_subdirectory(IWearLogger) 11 | endif() 12 | 13 | 14 | -------------------------------------------------------------------------------- /interfaces/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | add_subdirectory(IWear) 5 | add_subdirectory(IXsensMVNControl) 6 | add_subdirectory(IHumanState) 7 | add_subdirectory(IHumanWrench) 8 | add_subdirectory(IHumanDynamics) 9 | add_subdirectory(IWearableTargets) 10 | -------------------------------------------------------------------------------- /ci_env.yml: -------------------------------------------------------------------------------- 1 | name: human-dynamics-estimation 2 | channels: 3 | - conda-forge 4 | - robotology 5 | dependencies: 6 | - cmake 7 | - cxx-compiler 8 | - make 9 | - ninja 10 | - pkg-config 11 | - libyarp 12 | - libicub-main 13 | - idyntree 14 | - libmatio-cpp 15 | - librobometry 16 | - osqp-eigen 17 | - cmake-package-check 18 | - libtrintrin 19 | -------------------------------------------------------------------------------- /devices/FTShoes/src/FTShoes.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #include "FTShoes.h" 5 | 6 | using namespace wear::suit; 7 | 8 | class FTShoes::Impl 9 | {}; 10 | 11 | FTShoes::FTShoes() 12 | : pImpl{new Impl()} 13 | {} 14 | 15 | // Without this destructor here, the linker complains for 16 | // undefined reference to vtable 17 | FTShoes::~FTShoes() = default; 18 | -------------------------------------------------------------------------------- /XSensMVN/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | 5 | # Build the test unit executables 6 | # =============================== 7 | add_executable(testCalibrator ${CMAKE_CURRENT_SOURCE_DIR}/testCalibrator.cpp) 8 | target_link_libraries(testCalibrator XSensMVN) 9 | 10 | add_executable(testDriver ${CMAKE_CURRENT_SOURCE_DIR}/testDriver.cpp) 11 | target_link_libraries(testDriver XSensMVN) 12 | -------------------------------------------------------------------------------- /clients/HumanWrench_nwc_yarp/conf/HumanWrenchRemapperExample.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | /HDE/HumanWrench_nws_yarp/wrench:o 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /clients/HumanWrench_nwc_yarp/conf/HumanWrench_nwc_yarpExample.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | /HDE/HumanWrench_nws_yarp/wrench:o 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /clients/HumanDynamics_nwc_yarp/conf/HumanDynamics_nwc_yarpExample.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | /HDE/HumanDynamics_nws_yarp/torques:o 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /interfaces/IXsensMVNControl/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | 5 | # Group files with the correct path for the target_sources command 6 | file(GLOB_RECURSE IXSENS_MVN_CONTROL_HEADERS *.h) 7 | 8 | add_library(IXsensMVNControl INTERFACE) 9 | target_sources(IXsensMVNControl INTERFACE ${IXSENS_MVN_CONTROL_HEADERS}) 10 | target_include_directories(IXsensMVNControl INTERFACE 11 | $ 12 | ) 13 | -------------------------------------------------------------------------------- /conf/xml/TransformServer.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0.2 7 | 8 | true 9 | false 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /conf/app/HumanStateVisualizer.ini: -------------------------------------------------------------------------------- 1 | name HumanStateVisualizer 2 | 3 | # Model Configuration options 4 | modelURDFName "humanSubject01_66dof.urdf" 5 | ignoreMissingLinks true 6 | visualizeWrenches false 7 | 8 | # Camera options 9 | cameraDeltaPosition (2.0, 0.0, 0.5) 10 | useFixedCamera false # if set to false, the camera follows the model base link 11 | fixedCameraTarget (0.0, 0.0, 0.0) # this option is unused when useFixedCamera is false 12 | maxVisualizationFPS 65 13 | 14 | # Client Configuration 15 | humanStateDataPortName "/HDE/HumanStateServer/state:o" 16 | -------------------------------------------------------------------------------- /devices/HumanWrenchProvider/WrenchFrameTransformers.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | using namespace hde::devices::impl; 9 | 10 | bool IWrenchFrameTransformer::transformWrenchFrame(const iDynTree::Wrench inputWrench, 11 | iDynTree::Wrench& transformedWrench) 12 | { 13 | transformedWrench = transform * inputWrench; 14 | return true; 15 | } 16 | -------------------------------------------------------------------------------- /conf/app/HumanStateVisualizer_iCub3.ini: -------------------------------------------------------------------------------- 1 | name HumanStateVisualizer 2 | 3 | # Model Configuration options 4 | modelURDFName "teleoperation_iCub_model_V_3.urdf" 5 | ignoreMissingLinks true 6 | visualizeWrenches false 7 | 8 | # Camera options 9 | cameraDeltaPosition (0.0, 2.0, 0.5) 10 | useFixedCamera false # if set to false, the camera follows the model base link 11 | fixedCameraTarget (0.0, 0.0, 0.0) # this option is unused when useFixedCamera is false 12 | maxVisualizationFPS 65 13 | 14 | # Client Configuration 15 | humanStateDataPortName "/iCub/RobotStateServer/state:o" 16 | -------------------------------------------------------------------------------- /conf/app/HumanStateVisualizer_iCub2_5.ini: -------------------------------------------------------------------------------- 1 | name HumanStateVisualizer 2 | 3 | # Model Configuration options 4 | modelURDFName "teleoperation_iCub_model_V_2_5.urdf" 5 | ignoreMissingLinks true 6 | visualizeWrenches false 7 | 8 | # Camera options 9 | cameraDeltaPosition (0.0, 2.0, 0.5) 10 | useFixedCamera false # if set to false, the camera follows the model base link 11 | fixedCameraTarget (0.0, 0.0, 0.0) # this option is unused when useFixedCamera is false 12 | maxVisualizationFPS 65 13 | 14 | # Client Configuration 15 | humanStateDataPortName "/iCub/RobotStateServer/state:o" 16 | -------------------------------------------------------------------------------- /conf/ros/launch/iCubRviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /devices/Paexo/test-application/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | 5 | cmake_minimum_required(VERSION 3.16) 6 | project(PaexoTestApplication) 7 | 8 | find_package(IWear REQUIRED) 9 | find_package(WearableActuators REQUIRED) 10 | find_package(YARP COMPONENTS os REQUIRED) 11 | 12 | add_executable(${PROJECT_NAME} paexo-test-application.cpp) 13 | 14 | target_link_libraries(${PROJECT_NAME} PUBLIC 15 | IWear::IWear 16 | WearableActuators::WearableActuators 17 | YARP::YARP_os 18 | YARP::YARP_init) 19 | 20 | 21 | -------------------------------------------------------------------------------- /conf/ros/launch/iCub3Rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/feature_request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Feature request 3 | about: Suggest an idea for this project 4 | 5 | --- 6 | 7 | **Is your feature request related to a problem? Please describe.** 8 | A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] 9 | 10 | **Describe the solution you'd like** 11 | A clear and concise description of what you want to happen. 12 | 13 | **Describe alternatives you've considered** 14 | A clear and concise description of any alternative solutions or features you've considered. 15 | 16 | **Additional context** 17 | Add any other context or screenshots about the feature request here. 18 | -------------------------------------------------------------------------------- /HumanDynamicsEstimationLibrary/utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | find_package(OsqpEigen 0.4.0 REQUIRED) 5 | find_package(YARP 3.2 REQUIRED) 6 | find_package(iDynTree REQUIRED) 7 | 8 | add_component(NAME utils 9 | SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/iDynTreeUtils.cpp 10 | PUBLIC_HEADERS ${CMAKE_CURRENT_SOURCE_DIR}/include/hde/utils/iDynTreeUtils.hpp 11 | PUBLIC_LINK_LIBRARIES iDynTree::idyntree-core 12 | PRIVATE_LINK_LIBRARIES YARP::YARP_os 13 | Eigen3::Eigen 14 | ) 15 | -------------------------------------------------------------------------------- /devices/Paexo/conf/paexo-validation-motor-control.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # set n to 1 3 | min_motor_pos=30 4 | max_motor_pos=50 5 | step_size=2 6 | 7 | ## Paexo initialization 8 | echo "start" | yarp rpc /wearable/paexo/rpc:i 9 | sleep 0.5 10 | echo "en_bc_data" | yarp rpc /wearable/paexo/rpc:i 11 | sleep 0.5 12 | echo "init:r" | yarp rpc /wearable/paexo/rpc:i 13 | 14 | # Set max position as initial motor position 15 | current_pos=${max_motor_pos} 16 | 17 | # continue until $n equals 5 18 | while [ ${min_motor_pos} -le ${current_pos} ] 19 | do 20 | read -s -n1 key 21 | #sleep 0.1 22 | echo "move:r:${current_pos}" | yarp rpc /wearable/paexo/rpc:i 23 | current_pos=$((current_pos - step_size)) 24 | done 25 | -------------------------------------------------------------------------------- /interfaces/IHumanState/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | add_library(IHumanState INTERFACE) 5 | 6 | target_sources(IHumanState INTERFACE 7 | $ 8 | $) 9 | 10 | target_include_directories(IHumanState INTERFACE 11 | $ 12 | $) 13 | 14 | install( 15 | FILES hde/interfaces/IHumanState.h 16 | DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/hde/interfaces) 17 | 18 | -------------------------------------------------------------------------------- /interfaces/IHumanWrench/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | add_library(IHumanWrench INTERFACE) 5 | 6 | target_sources(IHumanWrench INTERFACE 7 | $ 8 | $) 9 | 10 | target_include_directories(IHumanWrench INTERFACE 11 | $ 12 | $) 13 | 14 | install( 15 | FILES hde/interfaces/IHumanWrench.h 16 | DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/hde/interfaces) 17 | 18 | -------------------------------------------------------------------------------- /impl/SensorsImpl/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | 5 | add_library(SensorsImpl 6 | SensorsImpl.cpp 7 | include/Wearable/IWear/Sensors/impl/SensorsImpl.h) 8 | add_library(Wearable::SensorsImpl ALIAS SensorsImpl) 9 | 10 | target_include_directories(SensorsImpl PUBLIC 11 | $) 12 | 13 | target_link_libraries(SensorsImpl PUBLIC IWear::IWear) 14 | 15 | install( 16 | TARGETS SensorsImpl 17 | EXPORT SensorsImpl 18 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 19 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 20 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) 21 | -------------------------------------------------------------------------------- /interfaces/IHumanDynamics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | add_library(IHumanDynamics INTERFACE) 5 | 6 | target_sources(IHumanDynamics INTERFACE 7 | $ 8 | $) 9 | 10 | target_include_directories(IHumanDynamics INTERFACE 11 | $ 12 | $) 13 | 14 | install( 15 | FILES hde/interfaces/IHumanDynamics.h 16 | DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/hde/interfaces) 17 | 18 | -------------------------------------------------------------------------------- /XSensMVN/include/XSensCalibrationQualities.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef XSENS_CALIBRATION_QUALITIES_H 5 | #define XSENS_CALIBRATION_QUALITIES_H 6 | 7 | #include 8 | 9 | namespace xsensmvn { 10 | 11 | enum CalibrationQuality 12 | { 13 | UNKNOWN = 0, // Unknown quality or not yet performed 14 | FAILED = 1, // Failed to retrieve a meaningful calibration 15 | POOR = 2, // Some serious issues, a new calibration is recommended 16 | ACCEPTABLE = 3, // Some anomalies, but nothing serious 17 | GOOD = 4, // Good resulting quality, no problems detected 18 | }; 19 | } 20 | 21 | #endif // XSENS_CALIBRATION_QUALITIES_H 22 | -------------------------------------------------------------------------------- /devices/FTShoes/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | 5 | add_library(FTShoes SHARED 6 | src/FTShoes.cpp 7 | include/FTShoes.h) 8 | 9 | target_include_directories(FTShoes 10 | PUBLIC $ 11 | $) 12 | 13 | target_link_libraries(FTShoes IWear) 14 | 15 | install(TARGETS FTShoes 16 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 17 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 18 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) 19 | 20 | install( 21 | FILES include/FTShoes.h 22 | DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/Wearable) 23 | -------------------------------------------------------------------------------- /interfaces/IHumanDynamics/hde/interfaces/IHumanDynamics.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef HDE_INTERFACES_IHUMANDYNAMICS 5 | #define HDE_INTERFACES_IHUMANDYNAMICS 6 | 7 | #include 8 | #include 9 | 10 | namespace hde { 11 | namespace interfaces { 12 | class IHumanDynamics; 13 | } // namespace interfaces 14 | } // namespace hde 15 | 16 | class hde::interfaces::IHumanDynamics 17 | { 18 | public: 19 | virtual ~IHumanDynamics() = default; 20 | 21 | virtual std::vector getJointNames() const = 0; 22 | virtual size_t getNumberOfJoints() const = 0; 23 | 24 | virtual std::vector getJointTorques() const = 0; 25 | }; 26 | 27 | #endif // HDE_INTERFACES_IHUMANDYNAMICS 28 | -------------------------------------------------------------------------------- /interfaces/IHumanWrench/hde/interfaces/IHumanWrench.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef HDE_INTERFACES_IHUMANWRENCH 5 | #define HDE_INTERFACES_IHUMANWRENCH 6 | 7 | #include 8 | #include 9 | 10 | namespace hde { 11 | namespace interfaces { 12 | class IHumanWrench; 13 | } // namespace interfaces 14 | } // namespace hde 15 | 16 | class hde::interfaces::IHumanWrench 17 | { 18 | public: 19 | virtual ~IHumanWrench() = default; 20 | 21 | virtual std::vector getWrenchSourceNames() const = 0; 22 | virtual size_t getNumberOfWrenchSources() const = 0; 23 | 24 | virtual std::vector getWrenches() const = 0; 25 | 26 | }; 27 | 28 | #endif // HDE_INTERFACES_IHUMANWRENCH 29 | -------------------------------------------------------------------------------- /devices/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | find_package(iDynTree) 5 | 6 | add_subdirectory(IWearRemapper) 7 | add_subdirectory(IAnalogSensorToIWear) 8 | add_subdirectory(IFrameTransformToIWear) 9 | 10 | if(ENABLE_Paexo) 11 | add_subdirectory(Paexo) 12 | endif() 13 | 14 | if(ENABLE_XsensSuit) 15 | add_subdirectory(XsensSuit) 16 | endif() 17 | 18 | if(ENABLE_ICub) 19 | add_subdirectory(ICub) 20 | endif() 21 | 22 | add_subdirectory(HumanStateProvider) 23 | add_subdirectory(HumanWrenchProvider) 24 | add_subdirectory(HumanDynamicsEstimator) 25 | add_subdirectory(HumanControlBoard) 26 | add_subdirectory(RobotPositionController) 27 | 28 | if(HUMANSTATEPROVIDER_ENABLE_LOGGER) 29 | add_subdirectory(HumanLogger) 30 | endif() 31 | -------------------------------------------------------------------------------- /app/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | # HDE xml files installation 5 | 6 | set (WEARABLES_XML_FILES xml/FTShoeLeftWearableDevice.xml 7 | xml/FTShoeRightWearableDevice.xml 8 | xml/FTShoesWearableDevice.xml 9 | xml/ICubWearableDevice.xml 10 | xml/SkinInsolesWearableDevice.xml 11 | xml/XsensSuitWearableDevice.xml 12 | xml/applications/WearableDevices-Dumper.xml 13 | xml/applications/HumanDynamicsEstimation-WearableDumper.xml 14 | xml/applications/WearableDevices.xml) 15 | 16 | install(FILES ${WEARABLES_XML_FILES} 17 | DESTINATION ${CMAKE_INSTALL_DATADIR}/HumanDynamicsEstimation) 18 | -------------------------------------------------------------------------------- /conf/app/HumanStateVisualizerWithDynamics.ini: -------------------------------------------------------------------------------- 1 | name HumanStateVisualizer 2 | 3 | # Model Configuration options 4 | modelURDFName "humanSubject01_66dof.urdf" 5 | ignoreMissingLinks true 6 | visualizeWrenches true 7 | 8 | # Camera options 9 | cameraDeltaPosition (2.0, 0.0, 0.5) 10 | useFixedCamera false # if set to false, the camera follows the model base link 11 | fixedCameraTarget (0.0, 0.0, 0.0) # this option is unused when useFixedCamera is false 12 | maxVisualizationFPS 65 13 | 14 | # Wrench Data Configuration 15 | forceScalingFactor 0.001 16 | wrenchSourceLinks (LeftFoot RightFoot) # link order should reflect WrenchServer port data 17 | 18 | # Client Configuration 19 | humanStateDataPortName "/HDE/HumanStateServer/state:o" 20 | humanWrenchServerPortName "/HDE/HumanStateServer/wrench:o" 21 | -------------------------------------------------------------------------------- /XSensMVN/include/XSensMVNDriverStates.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef XSENS_MVN_DRIVER_STATES_H 5 | #define XSENS_MVN_DRIVER_STATES_H 6 | 7 | /* Hack required to print verbose log messages when not in Release 8 | * It is not possible to directly use the NDEBUG variable since XSens defines regardless from the 9 | * compile option 10 | * This brings as consequence that the Logger should be included in the .h files ALWAYS before XSens 11 | * SDK files 12 | */ 13 | namespace xsens { 14 | enum class DriverStatus 15 | { 16 | Disconnected = 0, 17 | Scanning, 18 | Connected, 19 | Calibrating, 20 | CalibratedAndReadyToRecord, 21 | Recording, 22 | Unknown, 23 | }; 24 | } 25 | 26 | #endif // XSENS_MVN_DRIVER_STATES_H 27 | -------------------------------------------------------------------------------- /conf/xml/applications/HumanDynamicsEstimation.xml: -------------------------------------------------------------------------------- 1 | 2 | HumanDynamicsEstimation 3 | An application for running Human Dynamics Estimation (HDE) for only human or for physical Human-Robot interaction (pHRI) experiments 4 | 5 | 6 | HumanDynamicsEstimation-HumanDumper 7 | 8 | 9 | 10 | HumanDynamicsEstimation-HumanDynamics 11 | 12 | 13 | 14 | HumanDynamicsEstimation-HumanKinematics 15 | 16 | 17 | 18 | HumanDynamicsEstimation-pHRI 19 | 20 | 21 | 22 | XsensRetargetingPositionControlPole 23 | 24 | 25 | 26 | XsensRetargetingVisualization 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /wrappers/IWear/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | 5 | yarp_prepare_plugin(iwear_wrapper 6 | TYPE wearable::wrappers::IWearWrapper 7 | INCLUDE include/IWearWrapper.h 8 | CATEGORY device 9 | ADVANCED 10 | DEFAULT ON) 11 | 12 | yarp_add_plugin(IWearWrapper 13 | src/IWearWrapper.cpp 14 | include/IWearWrapper.h) 15 | 16 | target_include_directories(IWearWrapper PRIVATE 17 | $) 18 | 19 | target_link_libraries(IWearWrapper PUBLIC 20 | IWear trintrin::msgs YARP::YARP_dev) 21 | 22 | yarp_install( 23 | TARGETS IWearWrapper 24 | COMPONENT runtime 25 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 26 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 27 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 28 | -------------------------------------------------------------------------------- /devices/XsensSuit/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | 5 | yarp_prepare_plugin(xsens_suit 6 | TYPE wearable::devices::XsensSuit 7 | INCLUDE include/XsensSuit.h 8 | CATEGORY device 9 | ADVANCED 10 | DEFAULT ON) 11 | 12 | yarp_add_plugin(XsensSuit 13 | src/XsensSuit.cpp 14 | include/XsensSuit.h) 15 | 16 | target_include_directories(XsensSuit PRIVATE 17 | $) 18 | 19 | target_link_libraries(XsensSuit PUBLIC 20 | IWear::IWear IXsensMVNControl XSensMVN YARP::YARP_dev YARP::YARP_init) 21 | 22 | yarp_install( 23 | TARGETS XsensSuit 24 | COMPONENT runtime 25 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 26 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 27 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 28 | -------------------------------------------------------------------------------- /interfaces/IWear/include/Wearable/IWear/Common.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef WEARABLE_COMMON_H 5 | #define WEARABLE_COMMON_H 6 | 7 | #ifndef wError 8 | #include 9 | #define wError std::cerr 10 | #endif 11 | 12 | #ifndef wWarning 13 | #include 14 | #define wWarning std::cout 15 | #endif 16 | 17 | #include 18 | 19 | namespace wearable { 20 | 21 | const std::string Separator = "::"; 22 | 23 | enum class ElementType 24 | { 25 | WearableSensor = 0, 26 | WearableActuator, 27 | }; 28 | 29 | class IWearableDevice; 30 | 31 | } // namespace wearable 32 | 33 | class wearable::IWearableDevice 34 | { 35 | protected: 36 | ElementType m_wearable_element_type; 37 | 38 | virtual ElementType getWearableElementType() const = 0; 39 | }; 40 | 41 | #endif // WEARABLE_COMMON_H 42 | -------------------------------------------------------------------------------- /interfaces/IWearableTargets/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | add_library(IWearableTargets INTERFACE) 5 | 6 | find_package(iDynTree REQUIRED) 7 | 8 | target_sources(IWearableTargets INTERFACE 9 | $ 10 | $) 11 | 12 | target_include_directories(IWearableTargets INTERFACE 13 | $ 14 | $) 15 | 16 | target_link_libraries(IWearableTargets INTERFACE 17 | iDynTree::idyntree-core 18 | IWear::IWear) 19 | 20 | install( 21 | FILES hde/interfaces/IWearableTargets.h 22 | DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/hde/interfaces) 23 | 24 | -------------------------------------------------------------------------------- /conf/HDE-yarpmanager.ini: -------------------------------------------------------------------------------- 1 | # Path to the folder which contains application description files 2 | apppath = "xml/applications" 3 | load_subfolders = yes 4 | 5 | # Path to the folder which contains module description files 6 | #modpath = "xml/modules" 7 | 8 | # Path to the folder which contains module description files 9 | #respath = "xml/resources" 10 | 11 | # Fault tolerance 12 | # parameters: yes|No 13 | watchdog = no 14 | 15 | # Module failure awareness (if watchdog is enabled) 16 | # parameters: Ignore|terminate|prompt|recover 17 | module_failure = prompt 18 | 19 | # Connection failure awareness (if watchdog is enabled) 20 | # parameters: Ignore|terminate|prompt 21 | connection_failure = prompt 22 | 23 | # Automatically establish all connections 24 | # parameters: Yes|no 25 | auto_connect = no 26 | 27 | # Appearance (for yarpmanager) 28 | # parameters: No|dark|light 29 | color_theme = dark 30 | 31 | # External editor (e.g. gedit, notepad) 32 | external_editor = gvim 33 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help us improve 4 | 5 | --- 6 | 7 | **Describe the bug** 8 | A clear and concise description of what the bug is. 9 | 10 | **To Reproduce** 11 | Steps to reproduce the behavior: 12 | 1. Go to '...' 13 | 2. Click on '....' 14 | 3. Scroll down to '....' 15 | 4. See error 16 | 17 | **Expected behavior** 18 | A clear and concise description of what you expected to happen. 19 | 20 | **Screenshots** 21 | If applicable, add screenshots to help explain your problem. 22 | 23 | **Desktop (please complete the following information):** 24 | - OS: [e.g. iOS] 25 | - Browser [e.g. chrome, safari] 26 | - Version [e.g. 22] 27 | 28 | **Smartphone (please complete the following information):** 29 | - Device: [e.g. iPhone6] 30 | - OS: [e.g. iOS8.1] 31 | - Browser [e.g. stock browser, safari] 32 | - Version [e.g. 22] 33 | 34 | **Additional context** 35 | Add any other context about the problem here. 36 | -------------------------------------------------------------------------------- /app/Wearables-yarpmanager.ini: -------------------------------------------------------------------------------- 1 | # Path to the folder which contains application description files 2 | apppath = "xml/applications" 3 | load_subfolders = yes 4 | 5 | # Path to the folder which contains module description files 6 | #modpath = "xml/modules" 7 | 8 | # Path to the folder which contains module description files 9 | #respath = "xml/resources" 10 | 11 | # Fault tolerance 12 | # parameters: yes|No 13 | watchdog = no 14 | 15 | # Module failure awareness (if watchdog is enabled) 16 | # parameters: Ignore|terminate|prompt|recover 17 | module_failure = prompt 18 | 19 | # Connection failure awareness (if watchdog is enabled) 20 | # parameters: Ignore|terminate|prompt 21 | connection_failure = prompt 22 | 23 | # Automatically establish all connections 24 | # parameters: Yes|no 25 | auto_connect = no 26 | 27 | # Appearance (for yarpmanager) 28 | # parameters: No|dark|light 29 | color_theme = dark 30 | 31 | # External editor (e.g. gedit, notepad) 32 | external_editor = gvim 33 | -------------------------------------------------------------------------------- /devices/ICub/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | 5 | yarp_prepare_plugin(icub_wearable_device 6 | TYPE wearable::devices::ICub 7 | INCLUDE include/ICub.h 8 | CATEGORY device 9 | ADVANCED 10 | DEFAULT ON) 11 | 12 | yarp_add_plugin(ICub 13 | src/ICub.cpp 14 | include/ICub.h) 15 | 16 | target_include_directories(ICub PRIVATE 17 | $) 18 | 19 | target_link_libraries(ICub 20 | PUBLIC 21 | IWear::IWear 22 | YARP::YARP_dev 23 | YARP::YARP_init 24 | PRIVATE 25 | iDynTree::idyntree-model 26 | iDynTree::idyntree-high-level) 27 | 28 | yarp_install( 29 | TARGETS ICub 30 | COMPONENT runtime 31 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 32 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 33 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 34 | -------------------------------------------------------------------------------- /conf/app/HumanStateVisualizer_iCub2_5_openxr.ini: -------------------------------------------------------------------------------- 1 | name HumanStateVisualizer 2 | 3 | # Model Configuration options 4 | modelURDFName "model.urdf" 5 | ignoreMissingLinks true 6 | visualizeWrenches false 7 | visualizeFrames true 8 | visualizeTargets true 9 | 10 | # Camera options 11 | cameraDeltaPosition (0.0, 2.0, 0.5) 12 | useFixedCamera true # if set to false, the camera follows the model base link 13 | fixedCameraTarget (0.0, 0.0, 0.0) # this option is unused when useFixedCamera is false 14 | maxVisualizationFPS 65 15 | 16 | # Link visualization option 17 | visualizedLinksFrame (head, l_hand r_hand root_link ) 18 | linksFrameScalingFactor 0.1 19 | 20 | # Targets visualization option 21 | visualizedTargetsFrame (target_Head target_RightHand target_LeftHand) 22 | targetsFrameScalingFactor 0.2 23 | 24 | # Client Configuration 25 | humanStateDataPortName "/iCub/RobotStateServer/state:o" 26 | wearableTargetsServerPortName "/iCub/HDE/WearableTargetsServer/state:o" 27 | -------------------------------------------------------------------------------- /conf/app/HumanStateVisualizer_iRonCub_openxr.ini: -------------------------------------------------------------------------------- 1 | name HumanStateVisualizer 2 | 3 | # Model Configuration options 4 | modelURDFName "model.urdf" 5 | ignoreMissingLinks true 6 | visualizeWrenches false 7 | visualizeFrames true 8 | visualizeTargets true 9 | 10 | # Camera options 11 | cameraDeltaPosition (0.0, 2.0, 0.5) 12 | useFixedCamera true # if set to false, the camera follows the model base link 13 | fixedCameraTarget (0.0, 0.0, 0.0) # this option is unused when useFixedCamera is false 14 | maxVisualizationFPS 65 15 | 16 | # Link visualization option 17 | visualizedLinksFrame (head, r_elbow_1 l_elbow_1 root_link ) 18 | linksFrameScalingFactor 0.1 19 | 20 | # Targets visualization option 21 | visualizedTargetsFrame (target_Head target_RightHand target_LeftHand) 22 | targetsFrameScalingFactor 0.2 23 | 24 | # Client Configuration 25 | humanStateDataPortName "/iCub/RobotStateServer/state:o" 26 | wearableTargetsServerPortName "/iCub/HDE/WearableTargetsServer/state:o" 27 | -------------------------------------------------------------------------------- /devices/IAnalogSensorToIWear/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | 5 | yarp_prepare_plugin(ianalogsensor_to_iwear 6 | TYPE wearable::devices::IAnalogSensorToIWear 7 | INCLUDE include/IAnalogSensorToIWear.h 8 | CATEGORY device 9 | ADVANCED 10 | DEFAULT ON) 11 | 12 | yarp_add_plugin(IAnalogSensorToIWear 13 | src/IAnalogSensorToIWear.cpp 14 | include/IAnalogSensorToIWear.h) 15 | 16 | target_include_directories(IAnalogSensorToIWear PRIVATE 17 | $) 18 | 19 | target_link_libraries(IAnalogSensorToIWear PUBLIC 20 | IWear::IWear YARP::YARP_dev YARP::YARP_init) 21 | 22 | yarp_install( 23 | TARGETS IAnalogSensorToIWear 24 | COMPONENT runtime 25 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 26 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 27 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 28 | -------------------------------------------------------------------------------- /clients/HumanState_nwc_yarp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | yarp_prepare_plugin(human_state_nwc_yarp 5 | TYPE hde::devices::HumanState_nwc_yarp 6 | INCLUDE HumanState_nwc_yarp.h 7 | CATEGORY device 8 | ADVANCED 9 | DEFAULT ON) 10 | 11 | yarp_add_plugin(HumanState_nwc_yarp 12 | HumanState_nwc_yarp.cpp 13 | HumanState_nwc_yarp.h) 14 | 15 | target_include_directories(HumanState_nwc_yarp PUBLIC 16 | $) 17 | 18 | target_link_libraries(HumanState_nwc_yarp PUBLIC 19 | IHumanState 20 | trintrin::msgs 21 | YARP::YARP_os 22 | YARP::YARP_dev 23 | YARP::YARP_init) 24 | 25 | yarp_install( 26 | TARGETS HumanState_nwc_yarp 27 | COMPONENT runtime 28 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 29 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 30 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 31 | -------------------------------------------------------------------------------- /conf/app/HumanStateVisualizer_ergoCub_openxr.ini: -------------------------------------------------------------------------------- 1 | name HumanStateVisualizer 2 | 3 | # Model Configuration options 4 | modelURDFName "model.urdf" 5 | ignoreMissingLinks true 6 | visualizeWrenches false 7 | visualizeFrames true 8 | visualizeTargets true 9 | 10 | # Camera options 11 | cameraDeltaPosition (0.0, 2.0, 0.5) 12 | useFixedCamera true # if set to false, the camera follows the model base link 13 | fixedCameraTarget (0.0, 0.0, 0.0) # this option is unused when useFixedCamera is false 14 | maxVisualizationFPS 65 15 | 16 | # Link visualization option 17 | visualizedLinksFrame (r_hand_palm l_hand_palm root_link head) 18 | linksFrameScalingFactor 0.1 19 | 20 | # Targets visualization option 21 | visualizedTargetsFrame ( target_RightHand target_LeftHand target_Pelvis target_Head) 22 | targetsFrameScalingFactor 0.2 23 | 24 | # Client Configuration 25 | humanStateDataPortName "/ergoCub/RobotStateServer/state:o" 26 | wearableTargetsServerPortName "/HDE/WearableTargetsServer/state:o" 27 | -------------------------------------------------------------------------------- /servers/HumanState_nws_yarp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | yarp_prepare_plugin(human_state_nws_yarp 5 | TYPE hde::servers::HumanState_nws_yarp 6 | INCLUDE HumanState_nws_yarp.h 7 | CATEGORY device 8 | ADVANCED 9 | DEFAULT ON) 10 | 11 | yarp_add_plugin(HumanState_nws_yarp 12 | HumanState_nws_yarp.cpp 13 | HumanState_nws_yarp.h) 14 | 15 | target_include_directories(HumanState_nws_yarp PUBLIC 16 | $) 17 | 18 | target_link_libraries(HumanState_nws_yarp PUBLIC 19 | IHumanState 20 | trintrin::msgs 21 | YARP::YARP_os 22 | YARP::YARP_dev 23 | YARP::YARP_init) 24 | 25 | yarp_install( 26 | TARGETS HumanState_nws_yarp 27 | COMPONENT runtime 28 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 29 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 30 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 31 | -------------------------------------------------------------------------------- /devices/IWearRemapper/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | 5 | yarp_prepare_plugin(iwear_remapper 6 | TYPE wearable::devices::IWearRemapper 7 | INCLUDE include/IWearRemapper.h 8 | CATEGORY device 9 | ADVANCED 10 | DEFAULT ON) 11 | 12 | yarp_add_plugin(IWearRemapper 13 | src/IWearRemapper.cpp 14 | include/IWearRemapper.h) 15 | 16 | target_include_directories(IWearRemapper PUBLIC 17 | $) 18 | 19 | target_link_libraries(IWearRemapper PUBLIC 20 | IWear::IWear 21 | trintrin::msgs 22 | Wearable::SensorsImpl 23 | YARP::YARP_dev 24 | YARP::YARP_os 25 | YARP::YARP_init) 26 | 27 | yarp_install( 28 | TARGETS IWearRemapper 29 | COMPONENT runtime 30 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 31 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 32 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 33 | -------------------------------------------------------------------------------- /servers/HumanWrench_nws_yarp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | yarp_prepare_plugin(human_wrench_nws_yarp 5 | TYPE hde::servers::HumanWrench_nws_yarp 6 | INCLUDE HumanWrench_nws_yarp.h 7 | CATEGORY device 8 | ADVANCED 9 | DEFAULT ON) 10 | 11 | yarp_add_plugin(HumanWrench_nws_yarp 12 | HumanWrench_nws_yarp.cpp 13 | HumanWrench_nws_yarp.h) 14 | 15 | target_include_directories(HumanWrench_nws_yarp PUBLIC 16 | $) 17 | 18 | target_link_libraries(HumanWrench_nws_yarp PUBLIC 19 | IHumanWrench 20 | trintrin::msgs 21 | YARP::YARP_os 22 | YARP::YARP_dev 23 | YARP::YARP_init) 24 | 25 | yarp_install( 26 | TARGETS HumanWrench_nws_yarp 27 | COMPONENT runtime 28 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 29 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 30 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 31 | -------------------------------------------------------------------------------- /wrappers/IWearActuators/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | 5 | yarp_prepare_plugin(iwearactuators_wrapper 6 | TYPE wearable::wrappers::IWearActuatorsWrapper 7 | INCLUDE include/IWearActuatorsWrapper.h 8 | CATEGORY device 9 | ADVANCED 10 | DEFAULT ON) 11 | 12 | yarp_add_plugin(IWearActuatorsWrapper 13 | src/IWearActuatorsWrapper.cpp 14 | include/IWearActuatorsWrapper.h) 15 | 16 | target_include_directories(IWearActuatorsWrapper PRIVATE 17 | $) 18 | 19 | target_link_libraries(IWearActuatorsWrapper PUBLIC 20 | IWear trintrin::msgs YARP::YARP_dev YARP::YARP_os YARP::YARP_init) 21 | 22 | yarp_install( 23 | TARGETS IWearActuatorsWrapper 24 | COMPONENT runtime 25 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 26 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 27 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 28 | -------------------------------------------------------------------------------- /wrappers/IWearLogger/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | find_package(YARP COMPONENTS dev REQUIRED) 5 | find_package(robometry REQUIRED) 6 | 7 | yarp_prepare_plugin(iwear_logger 8 | TYPE wearable::wrappers::IWearLogger 9 | INCLUDE include/IWearLogger.h 10 | CATEGORY device 11 | ADVANCED 12 | DEFAULT ON) 13 | 14 | yarp_add_plugin(IWearLogger 15 | src/IWearLogger.cpp 16 | include/IWearLogger.h) 17 | 18 | target_include_directories(IWearLogger PRIVATE 19 | $) 20 | 21 | target_link_libraries(IWearLogger PUBLIC 22 | IWear trintrin::msgs YARP::YARP_dev YARP::YARP_init robometry::robometry) 23 | 24 | yarp_install( 25 | TARGETS IWearLogger 26 | COMPONENT runtime 27 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 28 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 29 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 30 | -------------------------------------------------------------------------------- /clients/HumanWrench_nwc_yarp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | yarp_prepare_plugin(human_wrench_nwc_yarp 5 | TYPE hde::devices::HumanWrench_nwc_yarp 6 | INCLUDE HumanWrench_nwc_yarp.h 7 | CATEGORY device 8 | ADVANCED 9 | DEFAULT ON) 10 | 11 | yarp_add_plugin(HumanWrench_nwc_yarp 12 | HumanWrench_nwc_yarp.cpp 13 | HumanWrench_nwc_yarp.h) 14 | 15 | target_include_directories(HumanWrench_nwc_yarp PUBLIC 16 | $) 17 | 18 | target_link_libraries(HumanWrench_nwc_yarp PUBLIC 19 | IHumanWrench 20 | trintrin::msgs 21 | YARP::YARP_os 22 | YARP::YARP_dev 23 | YARP::YARP_init) 24 | 25 | yarp_install( 26 | TARGETS HumanWrench_nwc_yarp 27 | COMPONENT runtime 28 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 29 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 30 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 31 | 32 | -------------------------------------------------------------------------------- /wrappers/IXsensMVNControl/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | 5 | yarp_prepare_plugin(ixsensmvncontrol_wrapper 6 | TYPE wearable::wrappers::IXsensMVNControlWrapper 7 | INCLUDE include/IXsensMVNControlWrapper.h 8 | CATEGORY device 9 | ADVANCED 10 | DEFAULT ON) 11 | 12 | yarp_add_plugin(IXsensMVNControlWrapper 13 | src/IXsensMVNControlWrapper.cpp 14 | include/IXsensMVNControlWrapper.h) 15 | 16 | target_include_directories(IXsensMVNControlWrapper PRIVATE 17 | $) 18 | 19 | target_link_libraries(IXsensMVNControlWrapper PUBLIC 20 | XsensSuitControl IXsensMVNControl YARP::YARP_dev) 21 | 22 | yarp_install( 23 | TARGETS IXsensMVNControlWrapper 24 | COMPONENT runtime 25 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 26 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 27 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 28 | -------------------------------------------------------------------------------- /devices/HumanWrenchProvider/WrenchFrameTransformers.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef HDE_DEVICES_IMPL_IWRENCHFRAMETRANSFORMERS 5 | #define HDE_DEVICES_IMPL_IWRENCHFRAMETRANSFORMERS 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | namespace hde { 14 | namespace devices { 15 | namespace impl { 16 | class IWrenchFrameTransformer; 17 | } // namespace impl 18 | } // namespace devices 19 | } // namespace hde 20 | 21 | class hde::devices::impl::IWrenchFrameTransformer 22 | { 23 | public: 24 | iDynTree::Transform transform; 25 | 26 | IWrenchFrameTransformer() = default; 27 | ~IWrenchFrameTransformer() = default; 28 | bool transformWrenchFrame(const iDynTree::Wrench inputWrench, 29 | iDynTree::Wrench& transformedWrench); 30 | }; 31 | 32 | #endif // HDE_DEVICES_IMPL_IWRENCHFRAMETRANSFORMERS 33 | -------------------------------------------------------------------------------- /servers/HumanDynamics_nws_yarp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | yarp_prepare_plugin(human_dynamics_nws_yarp 5 | TYPE hde::servers::HumanDynamics_nws_yarp 6 | INCLUDE HumanDynamics_nws_yarp.h 7 | CATEGORY device 8 | ADVANCED 9 | DEFAULT ON) 10 | 11 | yarp_add_plugin(HumanDynamics_nws_yarp 12 | HumanDynamics_nws_yarp.cpp 13 | HumanDynamics_nws_yarp.h) 14 | 15 | target_include_directories(HumanDynamics_nws_yarp PUBLIC 16 | $) 17 | 18 | target_link_libraries(HumanDynamics_nws_yarp PUBLIC 19 | IHumanDynamics 20 | trintrin::msgs 21 | YARP::YARP_os 22 | YARP::YARP_dev 23 | YARP::YARP_init) 24 | 25 | yarp_install( 26 | TARGETS HumanDynamics_nws_yarp 27 | COMPONENT runtime 28 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 29 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 30 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 31 | -------------------------------------------------------------------------------- /clients/HumanDynamics_nwc_yarp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | yarp_prepare_plugin(human_dynamics_nwc_yarp 5 | TYPE hde::devices::HumanDynamics_nwc_yarp 6 | INCLUDE HumanDynamics_nwc_yarp.h 7 | CATEGORY device 8 | ADVANCED 9 | DEFAULT ON) 10 | 11 | yarp_add_plugin(HumanDynamics_nwc_yarp 12 | HumanDynamics_nwc_yarp.cpp 13 | HumanDynamics_nwc_yarp.h) 14 | 15 | target_include_directories(HumanDynamics_nwc_yarp PUBLIC 16 | $) 17 | 18 | target_link_libraries(HumanDynamics_nwc_yarp PUBLIC 19 | IHumanDynamics 20 | trintrin::msgs 21 | YARP::YARP_os 22 | YARP::YARP_dev 23 | YARP::YARP_init) 24 | 25 | yarp_install( 26 | TARGETS HumanDynamics_nwc_yarp 27 | COMPONENT runtime 28 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 29 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 30 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 31 | 32 | -------------------------------------------------------------------------------- /clients/WearableTargets_nwc_yarp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | yarp_prepare_plugin(wearable_targets_nwc_yarp 5 | TYPE hde::devices::WearableTargets_nwc_yarp 6 | INCLUDE WearableTargets_nwc_yarp.h 7 | CATEGORY device 8 | ADVANCED 9 | DEFAULT ON) 10 | 11 | yarp_add_plugin(WearableTargets_nwc_yarp 12 | WearableTargets_nwc_yarp.cpp 13 | WearableTargets_nwc_yarp.h) 14 | 15 | target_include_directories(WearableTargets_nwc_yarp PUBLIC 16 | $) 17 | 18 | target_link_libraries(WearableTargets_nwc_yarp PUBLIC 19 | IWearableTargets 20 | trintrin::msgs 21 | YARP::YARP_os 22 | YARP::YARP_dev 23 | YARP::YARP_init) 24 | 25 | yarp_install( 26 | TARGETS WearableTargets_nwc_yarp 27 | COMPONENT runtime 28 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 29 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 30 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 31 | -------------------------------------------------------------------------------- /servers/WearableTargets_nws_yarp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | yarp_prepare_plugin(wearable_targets_nws_yarp 5 | TYPE hde::servers::WearableTargets_nws_yarp 6 | INCLUDE WearableTargets_nws_yarp.h 7 | CATEGORY device 8 | ADVANCED 9 | DEFAULT ON) 10 | 11 | yarp_add_plugin(WearableTargets_nws_yarp 12 | WearableTargets_nws_yarp.cpp 13 | WearableTargets_nws_yarp.h) 14 | 15 | target_include_directories(WearableTargets_nws_yarp PUBLIC 16 | $) 17 | 18 | target_link_libraries(WearableTargets_nws_yarp PUBLIC 19 | IWearableTargets 20 | trintrin::msgs 21 | YARP::YARP_os 22 | YARP::YARP_dev 23 | YARP::YARP_init) 24 | 25 | yarp_install( 26 | TARGETS WearableTargets_nws_yarp 27 | COMPONENT runtime 28 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 29 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 30 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 31 | -------------------------------------------------------------------------------- /interfaces/IWear/include/Wearable/IWear/Actuators/IHeater.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef WEARABLE_IHEATER_H 5 | #define WEARABLE_IHEATER_H 6 | 7 | #include "Wearable/IWear/Actuators/IActuator.h" 8 | 9 | namespace wearable { 10 | namespace actuator { 11 | class IHeater; 12 | } // namespace actuator 13 | } // namespace wearable 14 | 15 | class wearable::actuator::IHeater : public wearable::actuator::IActuator 16 | { 17 | public: 18 | IHeater(ActuatorName aName = {}, 19 | ActuatorStatus aStatus = ActuatorStatus::Unknown) 20 | : IActuator(aName, aStatus) 21 | { 22 | m_type = ActuatorType::Heater; 23 | } 24 | 25 | virtual ~IHeater() = default; 26 | 27 | inline static const std::string getPrefix(); 28 | 29 | //TODO: Update set and get methods 30 | }; 31 | 32 | inline const std::string wearable::actuator::IHeater::getPrefix() 33 | { 34 | return "heater" + wearable::Separator; 35 | } 36 | 37 | #endif // WEARABLE_IHEATER_H 38 | -------------------------------------------------------------------------------- /conf/app/HumanStateVisualizer_iRonCub_openxr_ifeel.ini: -------------------------------------------------------------------------------- 1 | name HumanStateVisualizer 2 | 3 | # Model Configuration options 4 | modelURDFName "model.urdf" 5 | ignoreMissingLinks true 6 | visualizeWrenches false 7 | visualizeFrames true 8 | visualizeTargets true 9 | 10 | # Camera options 11 | cameraDeltaPosition (0.0, 2.0, 0.5) 12 | useFixedCamera true # if set to false, the camera follows the model base link 13 | fixedCameraTarget (0.0, 0.0, 0.0) # this option is unused when useFixedCamera is false 14 | maxVisualizationFPS 65 15 | 16 | # Link visualization option 17 | visualizedLinksFrame (root_link l_shoulder_3 r_shoulder_3 head chest) 18 | linksFrameScalingFactor 0.1 19 | 20 | # Targets visualization option 21 | visualizedTargetsFrame ( target_RightHand target_LeftHand target_Pelvis target_Head target_Chest target_r_upper_arm target_l_upper_arm) 22 | targetsFrameScalingFactor 0.2 23 | 24 | # Client Configuration 25 | humanStateDataPortName "/iCub/RobotStateServer/state:o" 26 | wearableTargetsServerPortName "/iCub/HDE/WearableTargetsServer/state:o" 27 | -------------------------------------------------------------------------------- /interfaces/IWear/include/Wearable/IWear/Sensors/IGyroscope.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef WEARABLE_IGYROSCOPE_H 5 | #define WEARABLE_IGYROSCOPE_H 6 | 7 | #include "Wearable/IWear/Sensors/ISensor.h" 8 | 9 | namespace wearable { 10 | namespace sensor { 11 | class IGyroscope; 12 | } // namespace sensor 13 | } // namespace wearable 14 | 15 | class wearable::sensor::IGyroscope : public wearable::sensor::ISensor 16 | { 17 | public: 18 | IGyroscope(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) 19 | : ISensor(aName, aStatus) 20 | { 21 | m_type = SensorType::Gyroscope; 22 | } 23 | 24 | virtual ~IGyroscope() = default; 25 | 26 | virtual bool getAngularRate(Vector3& angularRate) const = 0; 27 | 28 | inline static const std::string getPrefix(); 29 | }; 30 | 31 | inline const std::string wearable::sensor::IGyroscope::getPrefix() 32 | { 33 | return "gyro" + wearable::Separator; 34 | } 35 | 36 | #endif // WEARABLE_IGYROSCOPE_H 37 | -------------------------------------------------------------------------------- /conf/xml/RobotPosePublisher.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0.01 7 | 0.1 8 | iCubGazeboV2_5.urdf 9 | root_link 10 | l_foot 11 | /Robot 12 | /Human/Pelvis 13 | /Human/LeftFoot 14 | 15 | (-1.0 0.0 0.0 0.0 -1.0 0.0 0.0 0.0 1.0) 16 | (0.6 -0.25 -0.095) 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /conf/app/HumanStateVisualizer_iCub3_openxr.ini: -------------------------------------------------------------------------------- 1 | name HumanStateVisualizer 2 | 3 | # Model Configuration options 4 | modelURDFName "teleoperation_iCub_model_V_3.urdf" 5 | ignoreMissingLinks true 6 | visualizeWrenches false 7 | visualizeFrames true 8 | visualizeTargets true 9 | 10 | # Camera options 11 | cameraDeltaPosition (0.0, 2.0, 0.5) 12 | useFixedCamera true # if set to false, the camera follows the model base link 13 | fixedCameraTarget (0.0, 0.0, 0.0) # this option is unused when useFixedCamera is false 14 | maxVisualizationFPS 65 15 | 16 | # Link visualization option 17 | visualizedLinksFrame (r_hand_fake l_hand_fake root_link l_upper_arm_fake l_forearm_fake r_upper_arm_fake r_forearm_fake head) 18 | linksFrameScalingFactor 0.1 19 | 20 | # Targets visualization option 21 | visualizedTargetsFrame ( target_RightHand target_LeftHand target_Pelvis target_Head) 22 | targetsFrameScalingFactor 0.2 23 | 24 | # Client Configuration 25 | humanStateDataPortName "/iCub/RobotStateServer/state:o" 26 | wearableTargetsServerPortName "/HDE/WearableTargetsServer/state:o" 27 | -------------------------------------------------------------------------------- /interfaces/IWear/include/Wearable/IWear/Sensors/IMagnetometer.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef WEARABLE_IMAGNETOMETER_H 5 | #define WEARABLE_IMAGNETOMETER_H 6 | 7 | #include "Wearable/IWear/Sensors/ISensor.h" 8 | 9 | namespace wearable { 10 | namespace sensor { 11 | class IMagnetometer; 12 | } // namespace sensor 13 | } // namespace wearable 14 | 15 | class wearable::sensor::IMagnetometer : public wearable::sensor::ISensor 16 | { 17 | public: 18 | IMagnetometer(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) 19 | : ISensor(aName, aStatus) 20 | { 21 | m_type = SensorType::Magnetometer; 22 | } 23 | 24 | virtual ~IMagnetometer() = default; 25 | 26 | virtual bool getMagneticField(Vector3& magneticField) const = 0; 27 | 28 | inline static const std::string getPrefix(); 29 | }; 30 | 31 | inline const std::string wearable::sensor::IMagnetometer::getPrefix() 32 | { 33 | return "mag" + wearable::Separator; 34 | } 35 | 36 | #endif // WEARABLE_IMAGNETOMETER_H 37 | -------------------------------------------------------------------------------- /interfaces/IWear/include/Wearable/IWear/Actuators/IMotor.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef WEARABLE_IMOTOR_H 5 | #define WEARABLE_IMOTOR_H 6 | 7 | #include "Wearable/IWear/Actuators/IActuator.h" 8 | 9 | namespace wearable { 10 | namespace actuator { 11 | class IMotor; 12 | } // namespace actuator 13 | } // namespace wearable 14 | 15 | class wearable::actuator::IMotor : public wearable::actuator::IActuator 16 | { 17 | public: 18 | IMotor(ActuatorName aName = {}, 19 | ActuatorStatus aStatus = ActuatorStatus::Unknown) 20 | : IActuator(aName, aStatus) 21 | { 22 | m_type = ActuatorType::Motor; 23 | } 24 | 25 | virtual ~IMotor() = default; 26 | 27 | inline static const std::string getPrefix(); 28 | 29 | virtual bool setMotorPosition(double& value) const = 0; //TODO: Double check if rads or deg 30 | }; 31 | 32 | inline const std::string wearable::actuator::IMotor::getPrefix() 33 | { 34 | return "motor" + wearable::Separator; 35 | } 36 | 37 | #endif // WEARABLE_IMOTOR_H 38 | -------------------------------------------------------------------------------- /interfaces/IWear/include/Wearable/IWear/Sensors/IForce3DSensor.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef WEARABLE_IFORCE_3D_SENSOR_H 5 | #define WEARABLE_IFORCE_3D_SENSOR_H 6 | 7 | #include "Wearable/IWear/Sensors/ISensor.h" 8 | 9 | namespace wearable { 10 | namespace sensor { 11 | class IForce3DSensor; 12 | } // namespace sensor 13 | } // namespace wearable 14 | 15 | class wearable::sensor::IForce3DSensor : public wearable::sensor::ISensor 16 | { 17 | public: 18 | IForce3DSensor(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) 19 | : ISensor(aName, aStatus) 20 | { 21 | m_type = SensorType::Force3DSensor; 22 | } 23 | 24 | virtual ~IForce3DSensor() = default; 25 | 26 | virtual bool getForce3D(Vector3& force) const = 0; 27 | 28 | inline static const std::string getPrefix(); 29 | }; 30 | 31 | inline const std::string wearable::sensor::IForce3DSensor::getPrefix() 32 | { 33 | return "f3D" + wearable::Separator; 34 | } 35 | 36 | #endif // WEARABLE_IFORCE_3D_SENSOR_H 37 | -------------------------------------------------------------------------------- /interfaces/IWear/include/Wearable/IWear/Sensors/ISkinSensor.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef WEARABLE_ISKIN_SENSOR_H 5 | #define WEARABLE_ISKIN_SENSOR_H 6 | 7 | #include "Wearable/IWear/Sensors/ISensor.h" 8 | #include 9 | 10 | namespace wearable { 11 | namespace sensor { 12 | class ISkinSensor; 13 | } // namespace sensor 14 | } // namespace wearable 15 | 16 | class wearable::sensor::ISkinSensor : public wearable::sensor::ISensor 17 | { 18 | public: 19 | ISkinSensor(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) 20 | : ISensor(aName, aStatus) 21 | { 22 | m_type = SensorType::SkinSensor; 23 | } 24 | 25 | virtual ~ISkinSensor() = default; 26 | 27 | virtual bool getPressure(std::vector& pressure) const = 0; 28 | 29 | inline static const std::string getPrefix(); 30 | }; 31 | 32 | inline const std::string wearable::sensor::ISkinSensor::getPrefix() 33 | { 34 | return "skin" + wearable::Separator; 35 | } 36 | 37 | #endif // WEARABLE_ISKIN_SENSOR_H 38 | -------------------------------------------------------------------------------- /interfaces/IWear/include/Wearable/IWear/Sensors/IAccelerometer.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef WEARABLE_IACCELEROMETER_H 5 | #define WEARABLE_IACCELEROMETER_H 6 | 7 | #include "Wearable/IWear/Sensors/ISensor.h" 8 | 9 | namespace wearable { 10 | namespace sensor { 11 | class IAccelerometer; 12 | } // namespace sensor 13 | } // namespace wearable 14 | 15 | class wearable::sensor::IAccelerometer : public wearable::sensor::ISensor 16 | { 17 | public: 18 | IAccelerometer(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) 19 | : ISensor(aName, aStatus) 20 | { 21 | m_type = SensorType::Accelerometer; 22 | } 23 | 24 | virtual ~IAccelerometer() = default; 25 | 26 | virtual bool getLinearAcceleration(Vector3& linearAcceleration) const = 0; 27 | 28 | inline static const std::string getPrefix(); 29 | }; 30 | 31 | inline const std::string wearable::sensor::IAccelerometer::getPrefix() 32 | { 33 | return "acc" + wearable::Separator; 34 | } 35 | #endif // WEARABLE_IACCELEROMETER_H 36 | -------------------------------------------------------------------------------- /interfaces/IWear/include/Wearable/IWear/Sensors/IPositionSensor.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef WEARABLE_IPOSITION_SENSOR_H 5 | #define WEARABLE_IPOSITION_SENSOR_H 6 | 7 | #include "Wearable/IWear/Sensors/ISensor.h" 8 | 9 | namespace wearable { 10 | namespace sensor { 11 | class IPositionSensor; 12 | } // namespace sensor 13 | } // namespace wearable 14 | 15 | class wearable::sensor::IPositionSensor : public wearable::sensor::ISensor 16 | { 17 | public: 18 | IPositionSensor(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) 19 | : ISensor(aName, aStatus) 20 | { 21 | m_type = SensorType::PositionSensor; 22 | } 23 | 24 | virtual ~IPositionSensor() = default; 25 | 26 | virtual bool getPosition(Vector3& position) const = 0; 27 | 28 | inline static const std::string getPrefix(); 29 | }; 30 | 31 | inline const std::string wearable::sensor::IPositionSensor::getPrefix() 32 | { 33 | return "pos" + wearable::Separator; 34 | } 35 | 36 | #endif // WEARABLE_IPOSITION_SENSOR_H 37 | -------------------------------------------------------------------------------- /interfaces/IWear/include/Wearable/IWear/Sensors/ITorque3DSensor.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef WEARABLE_ITORQUE_3D_SENSOR_H 5 | #define WEARABLE_ITORQUE_3D_SENSOR_H 6 | 7 | #include "Wearable/IWear/Sensors/ISensor.h" 8 | 9 | namespace wearable { 10 | namespace sensor { 11 | class ITorque3DSensor; 12 | } // namespace sensor 13 | } // namespace wearable 14 | 15 | class wearable::sensor::ITorque3DSensor : public wearable::sensor::ISensor 16 | { 17 | public: 18 | ITorque3DSensor(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) 19 | : ISensor(aName, aStatus) 20 | { 21 | m_type = SensorType::Torque3DSensor; 22 | } 23 | 24 | virtual ~ITorque3DSensor() = default; 25 | 26 | virtual bool getTorque3D(Vector3& torque) const = 0; 27 | 28 | inline static const std::string getPrefix(); 29 | }; 30 | 31 | inline const std::string wearable::sensor::ITorque3DSensor::getPrefix() 32 | { 33 | return "t3D" + wearable::Separator; 34 | } 35 | 36 | #endif // WEARABLE_ITORQUE_3D_SENSOR_H 37 | -------------------------------------------------------------------------------- /conf/app/HumanStateVisualizer_ergoCub_openxr_ifeel.ini: -------------------------------------------------------------------------------- 1 | name HumanStateVisualizer 2 | 3 | # Model Configuration options 4 | modelURDFName "model.urdf" 5 | ignoreMissingLinks true 6 | visualizeWrenches false 7 | visualizeFrames true 8 | visualizeTargets true 9 | 10 | # Camera options 11 | cameraDeltaPosition (0.0, 2.0, 0.5) 12 | useFixedCamera true # if set to false, the camera follows the model base link 13 | fixedCameraTarget (0.0, 0.0, 0.0) # this option is unused when useFixedCamera is false 14 | maxVisualizationFPS 65 15 | 16 | # Link visualization option 17 | visualizedLinksFrame (r_hand_palm l_hand_palm root_link l_upper_arm l_forearm r_upper_arm r_forearm head) 18 | linksFrameScalingFactor 0.1 19 | 20 | # Targets visualization option 21 | visualizedTargetsFrame ( target_RightHand target_LeftHand target_Pelvis target_l_upper_arm target_l_forearm target_r_upper_arm target_r_forearm target_Head) 22 | targetsFrameScalingFactor 0.2 23 | 24 | # Client Configuration 25 | humanStateDataPortName "/ergoCub/RobotStateServer/state:o" 26 | wearableTargetsServerPortName "/HDE/WearableTargetsServer/state:o" 27 | -------------------------------------------------------------------------------- /conf/app/HumanStateVisualizer_iCub2_5_openxr_ifeel.ini: -------------------------------------------------------------------------------- 1 | name HumanStateVisualizer 2 | 3 | # Model Configuration options 4 | modelURDFName "model.urdf" 5 | ignoreMissingLinks true 6 | visualizeWrenches false 7 | visualizeFrames true 8 | visualizeTargets true 9 | 10 | # Camera options 11 | cameraDeltaPosition (0.0, 2.0, 0.5) 12 | useFixedCamera true # if set to false, the camera follows the model base link 13 | fixedCameraTarget (0.0, 0.0, 0.0) # this option is unused when useFixedCamera is false 14 | maxVisualizationFPS 65 15 | 16 | # Link visualization option 17 | visualizedLinksFrame (root_link l_shoulder_3 l_forearm r_shoulder_3 r_forearm head chest) 18 | linksFrameScalingFactor 0.1 19 | 20 | # Targets visualization option 21 | visualizedTargetsFrame ( target_RightHand target_LeftHand target_Pelvis target_Head target_Chest target_r_upper_arm target_l_upper_arm target_r_forearm target_l_forearm) 22 | targetsFrameScalingFactor 0.2 23 | 24 | # Client Configuration 25 | humanStateDataPortName "/iCub/RobotStateServer/state:o" 26 | wearableTargetsServerPortName "/iCub/HDE/WearableTargetsServer/state:o" 27 | -------------------------------------------------------------------------------- /interfaces/IWear/include/Wearable/IWear/Sensors/IEmgSensor.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef WEARABLE_IEMG_SENSOR_H 5 | #define WEARABLE_IEMG_SENSOR_H 6 | 7 | #include "Wearable/IWear/Sensors/ISensor.h" 8 | 9 | namespace wearable { 10 | namespace sensor { 11 | class IEmgSensor; 12 | } // namespace sensor 13 | } // namespace wearable 14 | 15 | class wearable::sensor::IEmgSensor : public wearable::sensor::ISensor 16 | { 17 | public: 18 | IEmgSensor(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) 19 | : ISensor(aName, aStatus) 20 | { 21 | m_type = SensorType::EmgSensor; 22 | } 23 | 24 | virtual ~IEmgSensor() = default; 25 | 26 | virtual bool getEmgSignal(double& emgSignal) const = 0; 27 | virtual bool getNormalizationValue(double& normalizationValue) const = 0; 28 | 29 | inline static const std::string getPrefix(); 30 | }; 31 | 32 | inline const std::string wearable::sensor::IEmgSensor::getPrefix() 33 | { 34 | return "emg" + wearable::Separator; 35 | } 36 | #endif // WEARABLE_IEMG_SENSOR_H 37 | -------------------------------------------------------------------------------- /devices/HumanControlBoard/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | find_package(iDynTree REQUIRED) 5 | 6 | yarp_prepare_plugin(human_control_board 7 | TYPE hde::devices::HumanControlBoard 8 | INCLUDE HumanControlBoard.h 9 | CATEGORY device 10 | ADVANCED 11 | DEFAULT ON) 12 | 13 | yarp_add_plugin(HumanControlBoard 14 | HumanControlBoard.cpp 15 | HumanControlBoard.h) 16 | 17 | target_include_directories(HumanControlBoard PUBLIC 18 | $) 19 | 20 | add_definitions(-D_USE_MATH_DEFINES) 21 | 22 | target_link_libraries(HumanControlBoard PUBLIC 23 | IHumanState 24 | IHumanDynamics 25 | YARP::YARP_os 26 | YARP::YARP_dev 27 | YARP::YARP_init 28 | iDynTree::idyntree-model 29 | iDynTree::idyntree-estimation) 30 | 31 | yarp_install( 32 | TARGETS HumanControlBoard 33 | COMPONENT runtime 34 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 35 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 36 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 37 | -------------------------------------------------------------------------------- /interfaces/IWear/include/Wearable/IWear/Sensors/ITemperatureSensor.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef WEARABLE_ITEMPERATURE_SENSOR_H 5 | #define WEARABLE_ITEMPERATURE_SENSOR_H 6 | 7 | #include "Wearable/IWear/Sensors/ISensor.h" 8 | 9 | namespace wearable { 10 | namespace sensor { 11 | class ITemperatureSensor; 12 | } // namespace sensor 13 | } // namespace wearable 14 | 15 | class wearable::sensor::ITemperatureSensor : public wearable::sensor::ISensor 16 | { 17 | public: 18 | ITemperatureSensor(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) 19 | : ISensor(aName, aStatus) 20 | { 21 | m_type = SensorType::TemperatureSensor; 22 | } 23 | 24 | virtual ~ITemperatureSensor() = default; 25 | 26 | virtual bool getTemperature(double& temperature) const = 0; 27 | 28 | inline static const std::string getPrefix(); 29 | }; 30 | 31 | inline const std::string wearable::sensor::ITemperatureSensor::getPrefix() 32 | { 33 | return "temp" + wearable::Separator; 34 | } 35 | #endif // WEARABLE_ITEMPERATURE_SENSOR_H 36 | -------------------------------------------------------------------------------- /HumanDynamicsEstimationLibrary/algorithms/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | find_package(OsqpEigen 0.4.0 REQUIRED) 5 | find_package(YARP 3.2 REQUIRED) 6 | find_package(iDynTree REQUIRED) 7 | 8 | add_component(NAME algorithms 9 | SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/InverseVelocityKinematics.cpp 10 | ${CMAKE_CURRENT_SOURCE_DIR}/src/DynamicalInverseKinematics.cpp 11 | PUBLIC_HEADERS ${CMAKE_CURRENT_SOURCE_DIR}/include/hde/algorithms/InverseVelocityKinematics.hpp 12 | ${CMAKE_CURRENT_SOURCE_DIR}/include/hde/algorithms/DynamicalInverseKinematics.hpp 13 | PUBLIC_LINK_LIBRARIES iDynTree::idyntree-model 14 | PRIVATE_LINK_LIBRARIES YARP::YARP_os 15 | iDynTree::idyntree-high-level 16 | OsqpEigen::OsqpEigen 17 | osqp::osqp 18 | HumanDynamicsEstimation::utils 19 | ) 20 | 21 | if(MSVC) 22 | target_compile_options(algorithms PRIVATE /bigobj) 23 | endif() 24 | -------------------------------------------------------------------------------- /conf/app/HumanStateVisualizer_iCub3_openxr_ifeel.ini: -------------------------------------------------------------------------------- 1 | name HumanStateVisualizer 2 | 3 | # Model Configuration options 4 | modelURDFName "teleoperation_iCub_model_V_3.urdf" 5 | ignoreMissingLinks true 6 | visualizeWrenches false 7 | visualizeFrames true 8 | visualizeTargets true 9 | 10 | # Camera options 11 | cameraDeltaPosition (0.0, 2.0, 0.5) 12 | useFixedCamera true # if set to false, the camera follows the model base link 13 | fixedCameraTarget (0.0, 0.0, 0.0) # this option is unused when useFixedCamera is false 14 | maxVisualizationFPS 65 15 | 16 | # Link visualization option 17 | visualizedLinksFrame (r_hand_fake l_hand_fake root_link l_upper_arm_fake l_forearm_fake r_upper_arm_fake r_forearm_fake head) 18 | linksFrameScalingFactor 0.1 19 | 20 | # Targets visualization option 21 | visualizedTargetsFrame ( target_RightHand target_LeftHand target_Pelvis target_l_upper_arm_fake target_l_forearm_fake target_r_upper_arm_fake target_r_forearm_fake target_Head) 22 | targetsFrameScalingFactor 0.2 23 | 24 | # Client Configuration 25 | humanStateDataPortName "/iCub/RobotStateServer/state:o" 26 | wearableTargetsServerPortName "/HDE/WearableTargetsServer/state:o" 27 | -------------------------------------------------------------------------------- /devices/IFrameTransformToIWear/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | 5 | yarp_prepare_plugin(iframetransform_to_iwear 6 | TYPE wearable::devices::IFrameTransformToIWear 7 | INCLUDE include/IFrameTransformToIWear.h 8 | CATEGORY device 9 | ADVANCED 10 | DEFAULT ON) 11 | 12 | yarp_add_plugin(IFrameTransformToIWear 13 | src/IFrameTransformToIWear.cpp 14 | include/IFrameTransformToIWear.h) 15 | 16 | target_include_directories(IFrameTransformToIWear PRIVATE 17 | $) 18 | 19 | target_link_libraries(IFrameTransformToIWear PUBLIC 20 | IWear::IWear YARP::YARP_dev YARP::YARP_init) 21 | 22 | yarp_install( 23 | TARGETS IFrameTransformToIWear 24 | COMPONENT runtime 25 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 26 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 27 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 28 | 29 | set (WEARABLES_XML_FILES conf/iframetransform_to_iwear.xml) 30 | install(FILES ${WEARABLES_XML_FILES} 31 | DESTINATION ${CMAKE_INSTALL_DATADIR}/HumanDynamicsEstimation) 32 | -------------------------------------------------------------------------------- /devices/RobotPositionController/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | find_package(iDynTree REQUIRED) 5 | find_package(ICUB REQUIRED) 6 | 7 | yarp_prepare_plugin(robot_position_controller 8 | TYPE hde::devices::RobotPositionController 9 | INCLUDE RobotPositionController.h 10 | CATEGORY device 11 | ADVANCED 12 | DEFAULT ON) 13 | 14 | yarp_add_plugin(RobotPositionController 15 | RobotPositionController.cpp 16 | RobotPositionController.h) 17 | 18 | target_include_directories(RobotPositionController PUBLIC 19 | $) 20 | 21 | add_definitions(-D_USE_MATH_DEFINES) 22 | 23 | target_link_libraries(RobotPositionController PUBLIC 24 | IHumanState 25 | YARP::YARP_os 26 | YARP::YARP_dev 27 | YARP::YARP_init 28 | iDynTree::idyntree-model 29 | ctrlLib) 30 | 31 | yarp_install( 32 | TARGETS RobotPositionController 33 | COMPONENT runtime 34 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 35 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 36 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 37 | -------------------------------------------------------------------------------- /msgs/thrift/XsensSuitControlService.thrift: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | namespace yarp wearable.msg 5 | 6 | /** 7 | * Methods definition for the XsensDriver Wrapper service 8 | */ 9 | service XsensSuitControlService { 10 | 11 | /** 12 | * Calibrate the Xsens device with the default calibration procedure 13 | * @return true if the calibration is successful, false otherwise 14 | */ 15 | bool calibrate(); 16 | 17 | /** 18 | * Calibrate the Xsens device with the calibration procedure identified 19 | * by the specified parameter 20 | * 21 | * @param calibrationType name of the calibration to execute 22 | * @return true if the calibration is successful, false otherwise 23 | */ 24 | bool calibrateWithType(1: string calibrationType); 25 | 26 | /** 27 | * Abort the calibration procedure 28 | */ 29 | bool abortCalibration(); 30 | 31 | /** 32 | * Start acquiring data from the Xsens suit 33 | */ 34 | bool startAcquisition(); 35 | 36 | /** 37 | * Stop acquiring data from the suit 38 | */ 39 | bool stopAcquisition(); 40 | } 41 | -------------------------------------------------------------------------------- /devices/HumanLogger/HumanLogger.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef HUMANLOGGER_H 5 | #define HUMANLOGGER_H 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace hde { 13 | namespace devices { 14 | class HumanLogger; 15 | } // namespace devices 16 | } // namespace hde 17 | 18 | class hde::devices::HumanLogger 19 | : public yarp::dev::DeviceDriver 20 | , public yarp::dev::IMultipleWrapper 21 | , private yarp::os::PeriodicThread 22 | { 23 | private: 24 | class impl; 25 | std::unique_ptr pImpl; 26 | 27 | public: 28 | HumanLogger(); 29 | ~HumanLogger() override; 30 | 31 | // PeriodicThread 32 | void run() override; 33 | void threadRelease() override; 34 | 35 | // DeviceDriver interface 36 | bool open(yarp::os::Searchable& config) override; 37 | bool close() override; 38 | 39 | // IMultipleWrapper interface 40 | bool attachAll(const yarp::dev::PolyDriverList& driverList) override; 41 | bool detachAll() override; 42 | }; 43 | 44 | #endif // HUMANLOGGER_H 45 | -------------------------------------------------------------------------------- /interfaces/IWear/include/Wearable/IWear/Actuators/IHaptic.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef WEARABLE_IHAPTIC_H 5 | #define WEARABLE_IHAPTIC_H 6 | 7 | #include "Wearable/IWear/Actuators/IActuator.h" 8 | 9 | namespace wearable { 10 | namespace actuator { 11 | class IHaptic; 12 | } // namespace actuator 13 | } // namespace wearable 14 | 15 | class wearable::actuator::IHaptic : public wearable::actuator::IActuator 16 | { 17 | public: 18 | IHaptic(ActuatorName aName = {}, 19 | ActuatorStatus aStatus = ActuatorStatus::Unknown) 20 | : IActuator(aName, aStatus) 21 | { 22 | m_type = ActuatorType::Haptic; 23 | } 24 | 25 | virtual ~IHaptic() = default; 26 | 27 | inline static const std::string getPrefix(); 28 | 29 | virtual bool setHapticCommand(double& value) const = 0; 30 | 31 | virtual bool setHapticCommands(const std::vector& forceValue, const std::vector& vibrotactileValue) const = 0; 32 | }; 33 | 34 | inline const std::string wearable::actuator::IHaptic::getPrefix() 35 | { 36 | return "haptic" + wearable::Separator; 37 | } 38 | 39 | #endif // WEARABLE_IHAPTIC_H 40 | -------------------------------------------------------------------------------- /devices/HumanLogger/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | find_package(YARP COMPONENTS dev) 5 | find_package(robometry) 6 | 7 | 8 | yarp_prepare_plugin(human_logger 9 | TYPE hde::devices::HumanLogger 10 | INCLUDE HumanLogger.h 11 | CATEGORY device 12 | ADVANCED 13 | DEFAULT ON) 14 | 15 | yarp_add_plugin(HumanLogger 16 | HumanLogger.cpp 17 | HumanLogger.h) 18 | 19 | target_include_directories(HumanLogger PUBLIC 20 | $) 21 | 22 | target_link_libraries(HumanLogger PUBLIC 23 | YARP::YARP_os 24 | YARP::YARP_dev 25 | YARP::YARP_init 26 | robometry::robometry 27 | IHumanState 28 | IHumanDynamics 29 | IHumanWrench 30 | ) 31 | target_link_libraries(HumanLogger PRIVATE Eigen3::Eigen) 32 | 33 | 34 | 35 | if(MSVC) 36 | target_compile_options(HumanLogger PRIVATE /bigobj) 37 | endif() 38 | 39 | yarp_install( 40 | TARGETS HumanLogger 41 | COMPONENT runtime 42 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 43 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 44 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 45 | -------------------------------------------------------------------------------- /modules/IWearFrameVisualizer/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | 5 | # List the subdirectory 6 | # http://stackoverflow.com/questions/7787823/cmake-how-to-get-the-name-of-all-subdirectories-of-a-directory 7 | macro(SUBDIRLIST result curdir) 8 | file(GLOB children RELATIVE ${curdir} ${curdir}/*) 9 | set(dirlist "") 10 | foreach(child ${children}) 11 | if(IS_DIRECTORY ${curdir}/${child}) 12 | list(APPEND dirlist ${child}) 13 | endif() 14 | endforeach() 15 | set(${result} ${dirlist}) 16 | endmacro() 17 | 18 | 19 | set(EXE_TARGET_NAME IWearFrameVisualizerModule) 20 | 21 | find_package(YARP REQUIRED) 22 | find_package(iDynTree REQUIRED) 23 | 24 | add_executable(${EXE_TARGET_NAME} src/main.cpp) 25 | 26 | target_link_libraries(${EXE_TARGET_NAME} PUBLIC 27 | IWear 28 | YARP::YARP_os 29 | YARP::YARP_sig 30 | YARP::YARP_dev 31 | YARP::YARP_math 32 | iDynTree::idyntree-visualization 33 | ) 34 | 35 | install(TARGETS ${EXE_TARGET_NAME} DESTINATION bin) 36 | 37 | file(GLOB TARGET_INI_FILES conf/*.ini) 38 | 39 | install(FILES ${TARGET_INI_FILES} 40 | DESTINATION ${CMAKE_INSTALL_DATADIR}/HumanDynamicsEstimation) 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /interfaces/IWear/include/Wearable/IWear/Sensors/IFreeBodyAccelerationSensor.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef WEARABLE_IFREE_BODY_ACCELERATION_SENSOR_H 5 | #define WEARABLE_IFREE_BODY_ACCELERATION_SENSOR_H 6 | 7 | #include "Wearable/IWear/Sensors/ISensor.h" 8 | 9 | namespace wearable { 10 | namespace sensor { 11 | class IFreeBodyAccelerationSensor; 12 | } // namespace sensor 13 | } // namespace wearable 14 | 15 | class wearable::sensor::IFreeBodyAccelerationSensor : public wearable::sensor::ISensor 16 | { 17 | public: 18 | IFreeBodyAccelerationSensor(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) 19 | : ISensor(aName, aStatus) 20 | { 21 | m_type = SensorType::FreeBodyAccelerationSensor; 22 | } 23 | 24 | virtual ~IFreeBodyAccelerationSensor() = default; 25 | 26 | virtual bool getFreeBodyAcceleration(Vector3& freeBodyAcceleration) const = 0; 27 | 28 | inline static const std::string getPrefix(); 29 | }; 30 | 31 | inline const std::string wearable::sensor::IFreeBodyAccelerationSensor::getPrefix() 32 | { 33 | return "fbAcc" + wearable::Separator; 34 | } 35 | 36 | #endif // WEARABLE_IFREE_BODY_ACCELERATION_SENSOR_H 37 | -------------------------------------------------------------------------------- /interfaces/IHumanState/hde/interfaces/IHumanState.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef HDE_INTERFACES_IHUMANSTATE 5 | #define HDE_INTERFACES_IHUMANSTATE 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace hde { 12 | namespace interfaces { 13 | class IHumanState; 14 | } // namespace interfaces 15 | } // namespace hde 16 | 17 | class hde::interfaces::IHumanState 18 | { 19 | public: 20 | virtual ~IHumanState() = default; 21 | 22 | virtual std::vector getJointNames() const = 0; 23 | virtual std::string getBaseName() const = 0; 24 | virtual size_t getNumberOfJoints() const = 0; 25 | 26 | virtual std::vector getJointPositions() const = 0; 27 | virtual std::vector getJointVelocities() const = 0; 28 | 29 | virtual std::array getBasePosition() const = 0; 30 | virtual std::array getBaseOrientation() const = 0; 31 | 32 | virtual std::array getBaseVelocity() const = 0; 33 | 34 | virtual std::array getCoMPosition() const = 0; 35 | virtual std::array getCoMVelocity() const = 0; 36 | }; 37 | 38 | #endif // HDE_INTERFACES_IHUMANSTATE 39 | -------------------------------------------------------------------------------- /interfaces/IWear/include/Wearable/IWear/Sensors/IVirtualJointKinSensor.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef WEARABLE_IVIRTUAL_JOINT_KIN_SENSOR_H 5 | #define WEARABLE_IVIRTUAL_JOINT_KIN_SENSOR_H 6 | 7 | #include "Wearable/IWear/Sensors/ISensor.h" 8 | 9 | namespace wearable { 10 | namespace sensor { 11 | class IVirtualJointKinSensor; 12 | } // namespace sensor 13 | } // namespace wearable 14 | 15 | class wearable::sensor::IVirtualJointKinSensor : public wearable::sensor::ISensor 16 | { 17 | public: 18 | IVirtualJointKinSensor(SensorName aName = {}, 19 | SensorStatus aStatus = SensorStatus::Unknown) 20 | : ISensor(aName, aStatus) 21 | { 22 | m_type = SensorType::VirtualJointKinSensor; 23 | } 24 | 25 | virtual bool getJointPosition(double& position) const = 0; 26 | virtual bool getJointVelocity(double& velocity) const = 0; 27 | virtual bool getJointAcceleration(double& acceleration) const = 0; 28 | 29 | inline static const std::string getPrefix(); 30 | }; 31 | 32 | inline const std::string wearable::sensor::IVirtualJointKinSensor::getPrefix() 33 | { 34 | return "vJoint" + wearable::Separator; 35 | } 36 | 37 | 38 | #endif // WEARABLE_IVIRTUAL_JOINT_KIN_SENSOR_H 39 | -------------------------------------------------------------------------------- /devices/RobotPositionController/RobotPositionController.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef HDE_DEVICES_ROBOTPOSITIONCONTROLLER 5 | #define HDE_DEVICES_ROBOTPOSITIONCONTROLLER 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | namespace hde { 14 | namespace devices { 15 | class RobotPositionController; 16 | } // namespace devices 17 | } // namespace hde 18 | 19 | class hde::devices::RobotPositionController final 20 | : public yarp::dev::DeviceDriver 21 | , public yarp::dev::IWrapper 22 | , private yarp::os::PeriodicThread 23 | { 24 | private: 25 | class impl; 26 | std::unique_ptr pImpl; 27 | 28 | public: 29 | RobotPositionController(); 30 | ~RobotPositionController(); 31 | 32 | // DeviceDriver interface 33 | bool open(yarp::os::Searchable& config) override; 34 | bool close() override; 35 | 36 | // PeriodicThread 37 | void run() override; 38 | void threadRelease() override; 39 | 40 | // IWrapper interface 41 | bool attach(yarp::dev::PolyDriver* poly) override; 42 | bool detach() override; 43 | 44 | }; 45 | 46 | #endif // HDE_DEVICES_ROBOTPOSITIONCONTROLLER 47 | -------------------------------------------------------------------------------- /interfaces/IXsensMVNControl/IXsensMVNControl.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef IXSENS_MVN_CONTROL_H 5 | #define IXSENS_MVN_CONTROL_H 6 | 7 | #include 8 | #include 9 | 10 | namespace xsensmvn { 11 | class IXsensMVNControl; 12 | } 13 | 14 | class xsensmvn::IXsensMVNControl 15 | { 16 | public: 17 | /* -------------------------- * 18 | * Construtors / Destructors * 19 | * -------------------------- */ 20 | 21 | IXsensMVNControl() = default; 22 | virtual ~IXsensMVNControl() = default; 23 | 24 | /* ---------- * 25 | * Functions * 26 | * ---------- */ 27 | 28 | // Body dimensions set/get 29 | virtual bool setBodyDimensions(const std::map& dimensions) = 0; 30 | virtual bool getBodyDimensions(std::map& dimensions) const = 0; 31 | virtual bool getBodyDimension(const std::string bodyName, double& dimension) const = 0; 32 | 33 | // Calibration methods 34 | virtual bool calibrate(const std::string& calibrationType = {}) = 0; 35 | virtual bool abortCalibration() = 0; 36 | 37 | // Acquisition methods 38 | virtual bool startAcquisition() = 0; 39 | virtual bool stopAcquisition() = 0; 40 | }; 41 | 42 | #endif // IXSENS_MVN_CONTROL_H 43 | -------------------------------------------------------------------------------- /XSensMVN/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | 5 | set(XSENS_MVN_SOURCES 6 | ${CMAKE_CURRENT_SOURCE_DIR}/src/XSensMVNCalibrator.cpp 7 | ${CMAKE_CURRENT_SOURCE_DIR}/src/XSensMVNDriverImpl.cpp 8 | ${CMAKE_CURRENT_SOURCE_DIR}/src/XSensMVNDriver.cpp) 9 | 10 | set(XSENS_MVN_INCLUDES 11 | ${CMAKE_CURRENT_SOURCE_DIR}/include/XSensLogger.h 12 | ${CMAKE_CURRENT_SOURCE_DIR}/include/XSensCalibrationQualities.h 13 | ${CMAKE_CURRENT_SOURCE_DIR}/include/XSensMVNCalibrator.h 14 | ${CMAKE_CURRENT_SOURCE_DIR}/include/XSensMVNDriverImpl.h 15 | ${CMAKE_CURRENT_SOURCE_DIR}/include/XSensMVNDriver.h) 16 | 17 | add_library(XSensMVN 18 | ${XSENS_MVN_SOURCES} 19 | ${XSENS_MVN_INCLUDES}) 20 | 21 | target_include_directories(XSensMVN 22 | PUBLIC $ 23 | $) 24 | 25 | target_link_libraries(XSensMVN ${XsensXME_LIBRARIES} IXsensMVNControl) 26 | 27 | install(TARGETS XSensMVN 28 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 29 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 30 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) 31 | 32 | install( 33 | FILES ${XSENS_MVN_INCLUDES} 34 | DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/XSensMVN) 35 | 36 | add_subdirectory(test) 37 | -------------------------------------------------------------------------------- /devices/HumanWrenchProvider/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | find_package(iDynTree REQUIRED) 5 | find_package(Eigen3 REQUIRED) 6 | 7 | yarp_prepare_plugin(human_wrench_provider 8 | TYPE hde::devices::HumanWrenchProvider 9 | INCLUDE HumanWrenchProvider.h 10 | CATEGORY device 11 | ADVANCED 12 | DEFAULT ON) 13 | 14 | yarp_add_plugin(HumanWrenchProvider 15 | HumanWrenchProvider.cpp 16 | HumanWrenchProvider.h 17 | WrenchFrameTransformers.cpp 18 | WrenchFrameTransformers.h) 19 | 20 | target_include_directories(HumanWrenchProvider PUBLIC 21 | $) 22 | 23 | target_link_libraries(HumanWrenchProvider PUBLIC 24 | IHumanState 25 | IHumanWrench 26 | YARP::YARP_os 27 | YARP::YARP_dev 28 | YARP::YARP_sig 29 | YARP::YARP_init 30 | IWear::IWear 31 | iDynTree::idyntree-core 32 | iDynTree::idyntree-model 33 | iDynTree::idyntree-estimation 34 | iDynTree::idyntree-high-level) 35 | target_link_libraries(HumanWrenchProvider PRIVATE Eigen3::Eigen) 36 | 37 | yarp_install( 38 | TARGETS HumanWrenchProvider 39 | COMPONENT runtime 40 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 41 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 42 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 43 | -------------------------------------------------------------------------------- /wrappers/IWearLogger/include/IWearLogger.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef IWEARLOGGER_H 5 | #define IWEARLOGGER_H 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace wearable 14 | { 15 | namespace wrappers 16 | { 17 | class IWearLogger; 18 | } 19 | } // namespace wearable 20 | 21 | class wearable::wrappers::IWearLogger 22 | : public yarp::dev::DeviceDriver 23 | , public yarp::dev::IWrapper 24 | , public yarp::dev::IMultipleWrapper 25 | , public yarp::os::PeriodicThread 26 | { 27 | private: 28 | class impl; 29 | std::unique_ptr pImpl; 30 | 31 | public: 32 | IWearLogger(); 33 | ~IWearLogger() override; 34 | 35 | // PeriodicThread 36 | void run() override; 37 | void threadRelease() override; 38 | 39 | // DeviceDriver interface 40 | bool open(yarp::os::Searchable& config) override; 41 | bool close() override; 42 | 43 | // IWrapper interface 44 | bool attach(yarp::dev::PolyDriver* poly) override; 45 | bool detach() override; 46 | 47 | // IMultipleWrapper interface 48 | bool attachAll(const yarp::dev::PolyDriverList& driverList) override; 49 | bool detachAll() override; 50 | }; 51 | 52 | #endif // IWEARLOGGER_H 53 | -------------------------------------------------------------------------------- /wrappers/IWear/include/IWearWrapper.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef IWEARWRAPPER_H 5 | #define IWEARWRAPPER_H 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace wearable { 14 | namespace wrappers { 15 | class IWearWrapper; 16 | } 17 | } // namespace wearable 18 | 19 | class wearable::wrappers::IWearWrapper 20 | : public yarp::dev::DeviceDriver 21 | , public yarp::dev::IWrapper 22 | , public yarp::dev::IMultipleWrapper 23 | , public yarp::os::PeriodicThread 24 | { 25 | private: 26 | class impl; 27 | std::unique_ptr pImpl; 28 | 29 | public: 30 | IWearWrapper(); 31 | ~IWearWrapper() override; 32 | 33 | // PeriodicThread 34 | void run() override; 35 | void threadRelease() override; 36 | 37 | // DeviceDriver interface 38 | bool open(yarp::os::Searchable& config) override; 39 | bool close() override; 40 | 41 | // IWrapper interface 42 | bool attach(yarp::dev::PolyDriver* poly) override; 43 | bool detach() override; 44 | 45 | // IMultipleWrapper interface 46 | bool attachAll(const yarp::dev::PolyDriverList& driverList) override; 47 | bool detachAll() override; 48 | }; 49 | 50 | #endif // IWEARWRAPPER_H 51 | -------------------------------------------------------------------------------- /devices/HumanDynamicsEstimator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | find_package(iDynTree REQUIRED) 5 | find_package(Eigen3 REQUIRED) 6 | 7 | yarp_prepare_plugin(human_dynamics_estimator 8 | TYPE hde::devices::HumanDynamicsEstimator 9 | INCLUDE HumanDynamicsEstimator.h 10 | CATEGORY device 11 | ADVANCED 12 | DEFAULT ON) 13 | 14 | yarp_add_plugin(HumanDynamicsEstimator 15 | HumanDynamicsEstimator.cpp 16 | HumanDynamicsEstimator.h) 17 | 18 | target_include_directories(HumanDynamicsEstimator PUBLIC 19 | $) 20 | 21 | target_link_libraries(HumanDynamicsEstimator PUBLIC 22 | IHumanState 23 | IHumanWrench 24 | IHumanDynamics 25 | YARP::YARP_os 26 | YARP::YARP_dev 27 | YARP::YARP_sig 28 | YARP::YARP_init 29 | iDynTree::idyntree-core 30 | iDynTree::idyntree-model 31 | iDynTree::idyntree-estimation) 32 | target_link_libraries(HumanDynamicsEstimator PRIVATE Eigen3::Eigen) 33 | 34 | if(MSVC) 35 | target_compile_options(HumanDynamicsEstimator PRIVATE /bigobj) 36 | endif() 37 | 38 | yarp_install( 39 | TARGETS HumanDynamicsEstimator 40 | COMPONENT runtime 41 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 42 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 43 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 44 | -------------------------------------------------------------------------------- /modules/HumanStateVisualizer/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | # set target name 5 | set(EXE_TARGET_NAME HumanStateVisualizer) 6 | 7 | option(ENABLE_RPATH "Enable RPATH for this library" ON) 8 | mark_as_advanced(ENABLE_RPATH) 9 | include(AddInstallRPATHSupport) 10 | add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_BINDIR}" 11 | LIB_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}" 12 | INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}" 13 | DEPENDS ENABLE_RPATH 14 | USE_LINK_PATH) 15 | 16 | # Find required package 17 | find_package(YARP REQUIRED) 18 | find_package(iDynTree 12.0.0 REQUIRED) 19 | find_package(OsqpEigen 0.4.0 REQUIRED) 20 | 21 | # set cpp files 22 | set(${EXE_TARGET_NAME}_SRC 23 | src/main.cpp) 24 | 25 | # add include directories to the build. 26 | include_directories( 27 | ${CMAKE_CURRENT_SOURCE_DIR}/include 28 | ) 29 | 30 | # add an executable to the project using the specified source files. 31 | add_executable(${EXE_TARGET_NAME} ${${EXE_TARGET_NAME}_SRC} ) 32 | 33 | target_link_libraries(${EXE_TARGET_NAME} LINK_PUBLIC 34 | YARP::YARP_os 35 | YARP::YARP_dev 36 | YARP::YARP_init 37 | OsqpEigen::OsqpEigen 38 | iDynTree::idyntree-core 39 | iDynTree::idyntree-modelio 40 | iDynTree::idyntree-visualization 41 | IHumanState 42 | IWearableTargets 43 | IHumanWrench 44 | IHumanDynamics) 45 | 46 | install(TARGETS ${EXE_TARGET_NAME} DESTINATION bin) -------------------------------------------------------------------------------- /cmake/AddHDEPythonModule.cmake: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | function(add_hde_python_module) 5 | 6 | set(options ) 7 | set(oneValueArgs NAME) 8 | set(multiValueArgs 9 | SOURCES 10 | HEADERS 11 | LINK_LIBRARIES 12 | TESTS 13 | TESTS_RUNTIME_CONDITIONS) 14 | 15 | set(prefix "hde") 16 | 17 | cmake_parse_arguments(${prefix} 18 | "${options}" 19 | "${oneValueArgs}" 20 | "${multiValueArgs}" 21 | ${ARGN}) 22 | 23 | set(name ${${prefix}_NAME}) 24 | set(is_interface ${${prefix}_IS_INTERFACE}) 25 | set(sources ${${prefix}_SOURCES}) 26 | set(headers ${${prefix}_HEADERS}) 27 | set(link_libraries ${${prefix}_LINK_LIBRARIES}) 28 | set(subdirectories ${${prefix}_SUBDIRECTORIES}) 29 | set(tests ${${prefix}_TESTS}) 30 | set(tests_runtime_conditions ${${prefix}_TESTS_RUNTIME_CONDITIONS}) 31 | 32 | foreach(file ${headers}) 33 | set_property(GLOBAL APPEND PROPERTY pybind_headers ${CMAKE_CURRENT_SOURCE_DIR}/${file}) 34 | endforeach() 35 | 36 | foreach(file ${sources}) 37 | set_property(GLOBAL APPEND PROPERTY pybind_sources ${CMAKE_CURRENT_SOURCE_DIR}/${file}) 38 | endforeach() 39 | 40 | set_property(GLOBAL APPEND PROPERTY pybind_include_dirs ${CMAKE_CURRENT_SOURCE_DIR}/include) 41 | 42 | set_property(GLOBAL APPEND PROPERTY pybind_link_libraries ${link_libraries}) 43 | 44 | message(STATUS "Added files for bindings named ${name} in ${PROJECT_NAME}.") 45 | 46 | endfunction() 47 | -------------------------------------------------------------------------------- /conf/xml/applications/XsensRetargetingPositionControlPoleiCub3.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | XsensRetargetingPositionControlPoleiCub3 7 | Retargeting iCub On Pole using Xsens 8 | 1.0 9 | 10 | Kourosh Darvish 11 | 12 | 13 | 14 | 15 | 16 | yarprobotinterface 17 | --config TransformServer.xml 18 | localhost 19 | 20 | 21 | 22 | yarprobotstatepublisher 23 | --period 0.01 --tf-prefix /iCub/ --model teleoperation_iCub_model_V_3.urdf --reduced-model true --base-frame root_link --jointstates-topic "/iCub/joint_states" 24 | localhost 25 | 26 | 27 | 28 | roslaunch 29 | HDERviz iCub3Rviz.launch 30 | localhost 31 | 32 | 33 | 34 | yarprobotinterface 35 | --config RobotStateProvider_iCub3_Pole.xml 36 | localhost 37 | 38 | 39 | 40 | yarprobotinterface 41 | --config RobotPositionController_iCub.xml 42 | localhost 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /conf/xml/applications/XsensRetargetingPositionControlPoleiCub2_5.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | XsensRetargetingPositionControlPole 7 | Retargeting iCub On Pole using Xsens 8 | 1.0 9 | 10 | Kourosh Darvish 11 | 12 | 13 | 14 | 15 | 16 | yarprobotinterface 17 | --config TransformServer.xml 18 | localhost 19 | 20 | 21 | 22 | yarprobotstatepublisher 23 | --period 0.01 --tf-prefix /iCub/ --model teleoperation_iCub_model_V_2_5.urdf --reduced-model true --base-frame root_link_fake --jointstates-topic "/iCub/joint_states" 24 | localhost 25 | 26 | 27 | 28 | roslaunch 29 | HDERviz iCubRviz.launch 30 | localhost 31 | 32 | 33 | 34 | yarprobotinterface 35 | --config RobotStateProvider_iCub2_5_Pole.xml 36 | localhost 37 | 38 | 39 | 40 | yarprobotinterface 41 | --config RobotPositionController_iCub.xml 42 | localhost 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /cmake/AddWearablesPythonModule.cmake: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | function(add_wearables_python_module) 5 | 6 | set(options ) 7 | set(oneValueArgs NAME) 8 | set(multiValueArgs 9 | SOURCES 10 | HEADERS 11 | LINK_LIBRARIES 12 | TESTS 13 | TESTS_RUNTIME_CONDITIONS) 14 | 15 | set(prefix "wearables") 16 | 17 | cmake_parse_arguments(${prefix} 18 | "${options}" 19 | "${oneValueArgs}" 20 | "${multiValueArgs}" 21 | ${ARGN}) 22 | 23 | set(name ${${prefix}_NAME}) 24 | set(is_interface ${${prefix}_IS_INTERFACE}) 25 | set(sources ${${prefix}_SOURCES}) 26 | set(headers ${${prefix}_HEADERS}) 27 | set(link_libraries ${${prefix}_LINK_LIBRARIES}) 28 | set(subdirectories ${${prefix}_SUBDIRECTORIES}) 29 | set(tests ${${prefix}_TESTS}) 30 | set(tests_runtime_conditions ${${prefix}_TESTS_RUNTIME_CONDITIONS}) 31 | 32 | foreach(file ${headers}) 33 | set_property(GLOBAL APPEND PROPERTY pybind_headers ${CMAKE_CURRENT_SOURCE_DIR}/${file}) 34 | endforeach() 35 | 36 | foreach(file ${sources}) 37 | set_property(GLOBAL APPEND PROPERTY pybind_sources ${CMAKE_CURRENT_SOURCE_DIR}/${file}) 38 | endforeach() 39 | 40 | set_property(GLOBAL APPEND PROPERTY pybind_include_dirs ${CMAKE_CURRENT_SOURCE_DIR}/include) 41 | 42 | set_property(GLOBAL APPEND PROPERTY pybind_link_libraries ${link_libraries}) 43 | 44 | message(STATUS "Added files for bindings named ${name} in ${PROJECT_NAME}.") 45 | 46 | endfunction() 47 | -------------------------------------------------------------------------------- /devices/IAnalogSensorToIWear/conf/ianalogsensor_to_iwear.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0.5 6 | fakeSensor 7 | 8 | 9 | 10 | 11 | FakeSensor 12 | FakeWearable 13 | 1 14 | 0 15 | TemperatureSensor 16 | 17 | 18 | 19 | FakeAnalogSensor 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 0.01 29 | /Example/IWearWrapper/data:o 30 | /Example/IWearWrapper/metadata:o 31 | 32 | 33 | 34 | IAnalogSensorToIWear 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /servers/HumanState_nws_yarp/HumanState_nws_yarp.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef HDE_DEVICES_HUMANSTATE_NWS_YARP 5 | #define HDE_DEVICES_HUMANSTATE_NWS_YARP 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | namespace hde { 15 | namespace servers { 16 | class HumanState_nws_yarp; 17 | } // namespace servers 18 | } // namespace hde 19 | 20 | class hde::servers::HumanState_nws_yarp final 21 | : public yarp::dev::DeviceDriver 22 | , public yarp::dev::IWrapper 23 | , public yarp::dev::IMultipleWrapper 24 | , public yarp::os::PeriodicThread 25 | { 26 | private: 27 | class impl; 28 | std::unique_ptr pImpl; 29 | 30 | public: 31 | HumanState_nws_yarp(); 32 | ~HumanState_nws_yarp() override; 33 | 34 | // DeviceDriver interface 35 | bool open(yarp::os::Searchable& config) override; 36 | bool close() override; 37 | 38 | // PeriodicThread 39 | void run() override; 40 | void threadRelease() override; 41 | 42 | // IWrapper interface 43 | bool attach(yarp::dev::PolyDriver* poly) override; 44 | bool detach() override; 45 | 46 | // IMultipleWrapper interface 47 | bool attachAll(const yarp::dev::PolyDriverList& driverList) override; 48 | bool detachAll() override; 49 | }; 50 | 51 | #endif // HDE_DEVICES_HUMANSTATE_NWS_YARP 52 | -------------------------------------------------------------------------------- /servers/HumanWrench_nws_yarp/HumanWrench_nws_yarp.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef HDE_DEVICES_HUMANWRENCH_NWS_YARP 5 | #define HDE_DEVICES_HUMANWRENCH_NWS_YARP 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | namespace hde { 15 | namespace servers { 16 | class HumanWrench_nws_yarp; 17 | } // namespace servers 18 | } // namespace hde 19 | 20 | class hde::servers::HumanWrench_nws_yarp final 21 | : public yarp::dev::DeviceDriver 22 | , public yarp::dev::IWrapper 23 | , public yarp::dev::IMultipleWrapper 24 | , public yarp::os::PeriodicThread 25 | { 26 | private: 27 | class impl; 28 | std::unique_ptr pImpl; 29 | 30 | public: 31 | HumanWrench_nws_yarp(); 32 | ~HumanWrench_nws_yarp() override; 33 | 34 | // DeviceDriver interface 35 | bool open(yarp::os::Searchable& config) override; 36 | bool close() override; 37 | 38 | // PeriodicThread 39 | void run() override; 40 | void threadRelease() override; 41 | 42 | // IWrapper interface 43 | bool attach(yarp::dev::PolyDriver* poly) override; 44 | bool detach() override; 45 | 46 | // IMultipleWrapper interface 47 | bool attachAll(const yarp::dev::PolyDriverList& driverList) override; 48 | bool detachAll() override; 49 | 50 | }; 51 | 52 | #endif // HDE_DEVICES_HUMANWRENCH_NWS_YARP 53 | -------------------------------------------------------------------------------- /interfaces/IWear/include/Wearable/IWear/Sensors/IVirtualSphericalJointKinSensor.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef WEARABLE_IVIRTUAL_SPHERICAL_JOINT_KIN_SENSOR_H 5 | #define WEARABLE_IVIRTUAL_SPHERICAL_JOINT_KIN_SENSOR_H 6 | 7 | #include "Wearable/IWear/Sensors/ISensor.h" 8 | 9 | namespace wearable { 10 | namespace sensor { 11 | class IVirtualSphericalJointKinSensor; 12 | } // namespace sensor 13 | } // namespace wearable 14 | 15 | class wearable::sensor::IVirtualSphericalJointKinSensor : public wearable::sensor::ISensor 16 | { 17 | public: 18 | IVirtualSphericalJointKinSensor(SensorName aName = {}, 19 | SensorStatus aStatus = SensorStatus::Unknown) 20 | : ISensor(aName, aStatus) 21 | { 22 | m_type = SensorType::VirtualSphericalJointKinSensor; 23 | } 24 | 25 | virtual ~IVirtualSphericalJointKinSensor() = default; 26 | 27 | // CONVENTION: RPY are defined as rotations about X-Y-Z wrt fixed frame 28 | virtual bool getJointAnglesAsRPY(Vector3& angleAsRPY) const = 0; 29 | virtual bool getJointVelocities(Vector3& velocities) const = 0; 30 | virtual bool getJointAccelerations(Vector3& accelerations) const = 0; 31 | 32 | inline static const std::string getPrefix(); 33 | }; 34 | 35 | inline const std::string wearable::sensor::IVirtualSphericalJointKinSensor::getPrefix() 36 | { 37 | return "vSJoint" + wearable::Separator; 38 | } 39 | 40 | #endif // WEARABLE_IVIRTUAL_SPHERICAL_JOINT_KIN_SENSOR_H 41 | -------------------------------------------------------------------------------- /devices/HumanStateProvider/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | find_package(iDynTree REQUIRED) 5 | find_package(OsqpEigen 0.4.0 REQUIRED) 6 | 7 | 8 | yarp_prepare_plugin(human_state_provider 9 | TYPE hde::devices::HumanStateProvider 10 | INCLUDE HumanStateProvider.h 11 | CATEGORY device 12 | ADVANCED 13 | DEFAULT ON) 14 | 15 | yarp_add_plugin(HumanStateProvider 16 | HumanStateProvider.cpp 17 | HumanStateProvider.h 18 | IKWorkerPool.cpp 19 | IKWorkerPool.h) 20 | 21 | target_include_directories(HumanStateProvider PUBLIC 22 | $) 23 | 24 | target_link_libraries(HumanStateProvider PUBLIC 25 | YARP::YARP_os 26 | YARP::YARP_dev 27 | YARP::YARP_init 28 | IWear::IWear 29 | iDynTree::idyntree-model 30 | iDynTree::idyntree-inverse-kinematics 31 | iDynTree::idyntree-high-level 32 | OsqpEigen::OsqpEigen 33 | IHumanState 34 | IWearableTargets 35 | HumanDynamicsEstimation::utils 36 | HumanDynamicsEstimation::algorithms 37 | ) 38 | 39 | target_link_libraries(HumanStateProvider PRIVATE Eigen3::Eigen) 40 | 41 | 42 | if(MSVC) 43 | target_compile_options(HumanStateProvider PRIVATE /bigobj) 44 | endif() 45 | 46 | yarp_install( 47 | TARGETS HumanStateProvider 48 | COMPONENT runtime 49 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 50 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 51 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 52 | -------------------------------------------------------------------------------- /servers/WearableTargets_nws_yarp/WearableTargets_nws_yarp.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef HDE_DEVICES_WEARABLETARGETS_NWS_YARP 5 | #define HDE_DEVICES_WEARABLETARGETS_NWS_YARP 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | namespace hde { 15 | namespace servers { 16 | class WearableTargets_nws_yarp; 17 | } // namespace servers 18 | } // namespace hde 19 | 20 | class hde::servers::WearableTargets_nws_yarp final 21 | : public yarp::dev::DeviceDriver 22 | , public yarp::dev::IWrapper 23 | , public yarp::dev::IMultipleWrapper 24 | , public yarp::os::PeriodicThread 25 | { 26 | private: 27 | class impl; 28 | std::unique_ptr pImpl; 29 | 30 | public: 31 | WearableTargets_nws_yarp(); 32 | ~WearableTargets_nws_yarp() override; 33 | 34 | // DeviceDriver interface 35 | bool open(yarp::os::Searchable& config) override; 36 | bool close() override; 37 | 38 | // PeriodicThread 39 | void run() override; 40 | void threadRelease() override; 41 | 42 | // IWrapper interface 43 | bool attach(yarp::dev::PolyDriver* poly) override; 44 | bool detach() override; 45 | 46 | // IMultipleWrapper interface 47 | bool attachAll(const yarp::dev::PolyDriverList& driverList) override; 48 | bool detachAll() override; 49 | }; 50 | 51 | #endif // HDE_DEVICES_WEARABLETARGETS_NWS_YARP 52 | -------------------------------------------------------------------------------- /servers/HumanDynamics_nws_yarp/HumanDynamics_nws_yarp.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef HDE_DEVICES_HUMANDYNAMICS_NWS_YARP 5 | #define HDE_DEVICES_HUMANDYNAMICS_NWS_YARP 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | namespace hde { 15 | namespace servers { 16 | class HumanDynamics_nws_yarp; 17 | } // namespace servers 18 | } // namespace hde 19 | 20 | class hde::servers::HumanDynamics_nws_yarp final 21 | : public yarp::dev::DeviceDriver 22 | , public yarp::dev::IWrapper 23 | , public yarp::dev::IMultipleWrapper 24 | , public yarp::os::PeriodicThread 25 | { 26 | private: 27 | class impl; 28 | std::unique_ptr pImpl; 29 | 30 | public: 31 | HumanDynamics_nws_yarp(); 32 | ~HumanDynamics_nws_yarp() override; 33 | 34 | // DeviceDriver interface 35 | bool open(yarp::os::Searchable& config) override; 36 | bool close() override; 37 | 38 | // PeriodicThread 39 | void run() override; 40 | void threadRelease() override; 41 | 42 | // IWrapper interface 43 | bool attach(yarp::dev::PolyDriver* poly) override; 44 | bool detach() override; 45 | 46 | // IMultipleWrapper interface 47 | bool attachAll(const yarp::dev::PolyDriverList& driverList) override; 48 | bool detachAll() override; 49 | 50 | }; 51 | 52 | #endif // HDE_DEVICES_HUMANDYNAMICS_NWS_YARP 53 | -------------------------------------------------------------------------------- /wrappers/IXsensMVNControl/include/IXsensMVNControlWrapper.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef IXSENS_MVN_CONTROL_WRAPPER_H 5 | #define IXSENS_MVN_CONTROL_WRAPPER_H 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | //#include "IXsensMVNControl.h" 15 | 16 | namespace wearable { 17 | namespace wrappers { 18 | class IXsensMVNControlWrapper; 19 | } 20 | } // namespace wearable 21 | 22 | // using namespace xsensmvn; 23 | 24 | class wearable::wrappers::IXsensMVNControlWrapper 25 | : public yarp::dev::DeviceDriver 26 | , public yarp::dev::IWrapper 27 | , public yarp::dev::IMultipleWrapper 28 | , public yarp::os::PeriodicThread 29 | { 30 | private: 31 | class impl; 32 | std::unique_ptr pImpl; 33 | 34 | public: 35 | IXsensMVNControlWrapper(); 36 | ~IXsensMVNControlWrapper() override; 37 | 38 | // DeviceDriver interface 39 | bool open(yarp::os::Searchable& config) override; 40 | bool close() override; 41 | 42 | // IWrapper interface 43 | bool attach(yarp::dev::PolyDriver* poly) override; 44 | bool detach() override; 45 | 46 | // PeriodicThread 47 | void run() override; 48 | 49 | // IMultipleWrapper interface 50 | bool attachAll(const yarp::dev::PolyDriverList& driverList) override; 51 | bool detachAll() override; 52 | }; 53 | 54 | #endif // IXSENS_MVN_CONTROL_WRAPPER_H 55 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) Fondazione Istituto Italiano di Tecnologia (IIT) 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | -------------------------------------------------------------------------------- /conf/ros/launch/twoHumansRviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /app/xml/applications/HumanDynamicsEstimation-WearableDumper.xml: -------------------------------------------------------------------------------- 1 | 2 | HumanDumper 3 | Dumper for Wearable Data to run Human Dynamics Estimation 4 | 5 | localhost 6 | 7 | 8 | yarpdatadumper 9 | --name /human_dump/wearable/FTshoes/left --type bottle --txTime --rxTime 10 | ${generic_node} 11 | yarpdatadumper 12 | 13 | 14 | 15 | yarpdatadumper 16 | --name /human_dump/wearable/FTshoes/right --type bottle --txTime --rxTime 17 | ${generic_node} 18 | yarpdatadumper 19 | 20 | 21 | 22 | yarpdatadumper 23 | --name /human_dump/wearable/xsens --type bottle --txTime --rxTime 24 | ${generic_node} 25 | yarpdatadumper 26 | 27 | 28 | 29 | /FTShoeLeft/WearableData/data:o 30 | /human_dump/wearable/FTshoes/left 31 | udp 32 | 33 | 34 | 35 | /FTShoeRight/WearableData/data:o 36 | /human_dump/wearable/FTshoes/right 37 | udp 38 | 39 | 40 | 41 | /XSensSuit/WearableData/data:o 42 | /human_dump/wearable/xsens 43 | udp 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /conf/ros/launch/HDERviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /devices/HumanDynamicsEstimator/HumanDynamicsEstimator.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef HDE_DEVICES_HUMANDYNAMICSESTIMATOR 5 | #define HDE_DEVICES_HUMANDYNAMICSESTIMATOR 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | #include 14 | 15 | namespace hde { 16 | namespace devices { 17 | class HumanDynamicsEstimator; 18 | } // namespace devices 19 | } // namespace hde 20 | 21 | class hde::devices::HumanDynamicsEstimator final 22 | : public yarp::dev::DeviceDriver 23 | , public yarp::dev::IMultipleWrapper 24 | , private yarp::os::PeriodicThread 25 | , public hde::interfaces::IHumanDynamics 26 | { 27 | private: 28 | class Impl; 29 | std::unique_ptr pImpl; 30 | 31 | public: 32 | HumanDynamicsEstimator(); 33 | ~HumanDynamicsEstimator() override; 34 | 35 | // DeviceDriver interface 36 | bool open(yarp::os::Searchable& config) override; 37 | bool close() override; 38 | 39 | // PeriodicThread 40 | void run() override; 41 | void threadRelease() override; 42 | 43 | // IMultipleWrapper interface 44 | bool attachAll(const yarp::dev::PolyDriverList& driverList) override; 45 | bool detachAll() override; 46 | 47 | // IHumanDynamics 48 | std::vector getJointNames() const override; 49 | size_t getNumberOfJoints() const override; 50 | std::vector getJointTorques() const override; 51 | }; 52 | 53 | #endif // HDE_DEVICES_HUMANDYNAMICSESTIMATOR 54 | -------------------------------------------------------------------------------- /devices/ICub/conf/ICub.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | model.urdf 5 | root_link 6 | 7 | /wholeBodyDynamics/left_arm/endEffectorWrench:o 8 | /wholeBodyDynamics/right_arm/endEffectorWrench:o 9 | /wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o 10 | /wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o 11 | 12 | 13 | (head torso left_arm right_arm left_leg right_leg) 14 | /icubSim 15 | /ICubWearableRemoteClient 16 | 17 | 18 | 19 | 20 | 0.01 21 | /ICub/WearableData/data:o 22 | /ICub/WearableData/metadataRpc:o 23 | 24 | 25 | ICubWearableDevice 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /cmake/FindSenseGlove.cmake: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | # Finds the Sense Glove SDK 5 | # 6 | # This will define the following variables:: 7 | # 8 | # SenseGlove_FOUND - True if the system has the Sense Glove SDK 9 | # SenseGlove - The name of the libraries to link against. 10 | ############################### 11 | 12 | # Check Directory of the SenseGlove_DIR 13 | if(NOT DEFINED ENV{SenseGlove_DIR}) 14 | message( FATAL_ERROR "Environment variable {SenseGlove_DIR} is not defined." ) 15 | else() 16 | message(STATUS "Environment variable {SenseGlove_DIR}: $ENV{SenseGlove_DIR}" ) 17 | endif() 18 | 19 | if (CMAKE_BUILD_TYPE MATCHES "Debug") 20 | set(BUILD_TYPE "Debug") 21 | else() 22 | set(BUILD_TYPE "Release") 23 | endif() 24 | message(STATUS "SenseGlove is linking to BUILD_TYPE: ${BUILD_TYPE}") 25 | 26 | 27 | if(WIN32) 28 | file(GLOB SenseGlove_LIB $ENV{SenseGlove_DIR}/Core/SGCoreCpp/lib/win/${BUILD_TYPE}/SGCoreCpp.lib ) 29 | set(LIB_TYPE "STATIC") 30 | else() 31 | file(GLOB SenseGlove_LIB $ENV{SenseGlove_DIR}/Core/SGCoreCpp/lib/linux/${BUILD_TYPE}/libSGCoreCpp.so ) 32 | set(LIB_TYPE "SHARED") 33 | endif() 34 | 35 | set(SenseGlove_INCLUDE_DIRS $ENV{SenseGlove_DIR}/Core/SGCoreCpp/incl ) 36 | 37 | message(STATUS "Variable {SenseGlove_LIB}: ${SenseGlove_LIB}" ) 38 | message(STATUS "variable {SenseGlove_INCLUDE_DIRS}: ${SenseGlove_INCLUDE_DIRS}" ) 39 | 40 | ##### Find SenseGlove ##### 41 | 42 | add_library(SenseGlove ${LIB_TYPE} IMPORTED GLOBAL ${SenseGlove_LIB}) 43 | set_target_properties(SenseGlove PROPERTIES IMPORTED_LOCATION ${SenseGlove_LIB}) 44 | target_include_directories(SenseGlove INTERFACE ${SenseGlove_INCLUDE_DIRS}) 45 | 46 | set(SenseGlove_FOUND TRUE) 47 | -------------------------------------------------------------------------------- /conf/xml/applications/XsensRetargetingVisualizationiCub3.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | XsensRetargetingVisualization iCub3 8 | visualization of IK for retargeting 9 | 1.0 10 | 11 | Kourosh Darvish 12 | 13 | 14 | 15 | 16 | 17 | yarprobotinterface 18 | --config TransformServer.xml 19 | localhost 20 | 21 | 22 | 23 | yarprobotinterface 24 | --config RobotStateProvider_iCub3.xml 25 | localhost 26 | 27 | 28 | 29 | 30 | HumanStateVisualizer 31 | --from HumanStateVisualizer_iCub3.ini 32 | 33 | /iCub/RobotStateServer/state:o 34 | 35 | YARP_FORWARD_LOG_ENABLE=1 36 | Run the iDynTree Visualizer 37 | localhost 38 | 39 | 40 | 41 | yarprobotstatepublisher 42 | --period 0.01 --tf-prefix /iCub/ --model teleoperation_iCub_model_V_3.urdf --reduced-model true --base-frame root_link --jointstates-topic "/iCub/joint_states" 43 | localhost 44 | 45 | 46 | 47 | roslaunch 48 | HDERviz iCub3Rviz.launch 49 | localhost 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /app/xml/ICubWearableDevice.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model.urdf 6 | root_link 7 | 8 | /wholeBodyDynamics/left_arm/endEffectorWrench:o 9 | /wholeBodyDynamics/right_arm/endEffectorWrench:o 10 | /wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o 11 | /wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o 12 | 13 | 14 | (head torso left_arm right_arm left_leg right_leg) 15 | /icubSim 16 | /ICubWearableRemoteClient 17 | 18 | 19 | 20 | 21 | 0.01 22 | /ICub/WearableData/data:o 23 | /ICub/WearableData/metadataRpc:o 24 | 25 | 26 | ICubWearableDevice 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /clients/WearableTargets_nwc_yarp/WearableTargets_nwc_yarp.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef HDE_DEVICES_WEARABLETARGETS_NWC_YARP 5 | #define HDE_DEVICES_WEARABLETARGETS_NWC_YARP 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | namespace trintrin::msgs { 17 | class WearableTargets; 18 | } // namespace trintrin::msgs 19 | namespace hde::devices { 20 | class WearableTargets_nwc_yarp; 21 | } // namespace hde::devices 22 | 23 | class hde::devices::WearableTargets_nwc_yarp final 24 | : public yarp::dev::DeviceDriver 25 | , public hde::interfaces::IWearableTargets 26 | , public yarp::os::TypedReaderCallback 27 | , public yarp::os::PeriodicThread 28 | { 29 | private: 30 | class impl; 31 | std::unique_ptr pImpl; 32 | 33 | public: 34 | WearableTargets_nwc_yarp(); 35 | ~WearableTargets_nwc_yarp() override; 36 | 37 | // DeviceDriver interface 38 | bool open(yarp::os::Searchable& config) override; 39 | bool close() override; 40 | 41 | // PeriodicThread 42 | void run() override; 43 | void threadRelease() override; 44 | 45 | // TypedReaderCallback 46 | void onRead(trintrin::msgs::WearableTargets& wearableTargetsMgs) override; 47 | 48 | // IWearableTargets interface 49 | std::vector getAllTargetsName() const override; 50 | std::shared_ptr getTarget(const TargetName name) const override; 51 | }; 52 | 53 | #endif // HDE_DEVICES_WEARABLETARGETS_NWC_YARP 54 | -------------------------------------------------------------------------------- /conf/xml/applications/XsensRetargetingVisualizationiCub2_5.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | XsensRetargetingVisualization iCub2.5 8 | visualization of IK for retargeting 9 | 1.0 10 | 11 | Kourosh Darvish 12 | 13 | 14 | 15 | 16 | 17 | yarprobotinterface 18 | --config TransformServer.xml 19 | localhost 20 | 21 | 22 | 23 | yarprobotinterface 24 | --config RobotStateProvider_iCub2_5.xml 25 | localhost 26 | 27 | 28 | 29 | 30 | HumanStateVisualizer 31 | --from HumanStateVisualizer_iCub2_5.ini 32 | 33 | /iCub/RobotStateServer/state:o 34 | 35 | YARP_FORWARD_LOG_ENABLE=1 36 | Run the iDynTree Visualizer 37 | localhost 38 | 39 | 40 | 41 | 42 | yarprobotstatepublisher 43 | --period 0.01 --tf-prefix /iCub/ --model teleoperation_iCub_model_V_2_5.urdf --reduced-model true --base-frame root_link_fake --jointstates-topic "/iCub/joint_states" 44 | localhost 45 | 46 | 47 | 48 | roslaunch 49 | HDERviz iCubRviz.launch 50 | localhost 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /devices/IFrameTransformToIWear/conf/iframetransform_to_iwear.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0.01 7 | ftc_yarp_only.xml 8 | /IFrameTransformToIWear/tf 9 | /IFrameTransformToIWear/tf/local_rpc 10 | 11 | 12 | 13 | 14 | TransformServer 15 | ground 16 | PoseSensor 17 | (ground /iCub/root_link_fake) 18 | 19 | 20 | 21 | TransformClient 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 0.01 31 | /Wearable/FrameTransform/data:o 32 | /Example/IWearWrapper/metadata:o 33 | 34 | 35 | 36 | IFrameTransformToIWear 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /devices/Paexo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | # Flag to enable iFeelDriver in Paexo wearable device 5 | option(ENABLE_PAEXO_USE_iFEELDriver "Flag that enables building Paexo wearable device with iFeelDriver" OFF) 6 | 7 | if (ENABLE_PAEXO_USE_iFEELDriver) 8 | 9 | ## Find iFeelDriver 10 | find_package(iFeelDriver REQUIRED) 11 | 12 | endif() 13 | 14 | # Generate cmake config.file 15 | configure_file ( 16 | "${CMAKE_CURRENT_SOURCE_DIR}/conf/paexo-cmake-config.h.in" 17 | "${CMAKE_CURRENT_BINARY_DIR}/Autogenerated/paexo/paexo-cmake-config.h" 18 | @ONLY) 19 | 20 | # Compile the plugin by default 21 | yarp_prepare_plugin(paexo TYPE wearable::devices::Paexo 22 | INCLUDE include/Paexo.h 23 | CATEGORY device 24 | ADVANCED 25 | DEFAULT ON 26 | ) 27 | 28 | yarp_add_plugin(Paexo src/Paexo.cpp include/Paexo.h) 29 | 30 | target_include_directories(Paexo PRIVATE 31 | $ 32 | $) 33 | 34 | target_link_libraries(Paexo 35 | PUBLIC 36 | YARP::YARP_dev 37 | YARP::YARP_init 38 | IWear::IWear 39 | ) 40 | 41 | if (ENABLE_PAEXO_USE_iFEELDriver) 42 | target_link_libraries(Paexo PUBLIC iFeelDriver::iFeelDriver) 43 | message(STATUS "Paexo: Compiling with iFeelDriver") 44 | endif() 45 | 46 | yarp_install(TARGETS Paexo 47 | COMPONENT runtime 48 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 49 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 50 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR} 51 | ) 52 | -------------------------------------------------------------------------------- /msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | # TODO: these libraries are enforced to be static. In order to let the devices 5 | # find them on Windows from the build directory, their Release / Debug 6 | # folder should be added to the PATH. However, this does not work as expected. 7 | 8 | # ======================= 9 | # XsensSuitControlService 10 | # ======================= 11 | 12 | yarp_add_idl(XSENSSUITCONTROL thrift/XsensSuitControlService.thrift) 13 | 14 | add_library(XsensSuitControl ${XSENSSUITCONTROL} thrift/XsensSuitControlService.thrift) 15 | target_link_libraries(XsensSuitControl YARP::YARP_os) 16 | 17 | # Extract the include directory from the files names 18 | foreach(file ${XSENSSUITCONTROL}) 19 | STRING(REGEX MATCH ".+\\.h?h$" file ${file}) 20 | if(file) 21 | get_filename_component(include_dir ${file} DIRECTORY) 22 | list(APPEND XSENSSUITCONTROL_INCLUDE_DIRS ${include_dir}) 23 | list(REMOVE_DUPLICATES XSENSSUITCONTROL_INCLUDE_DIRS) 24 | endif() 25 | endforeach() 26 | 27 | foreach(dir ${XSENSSUITCONTROL_INCLUDE_DIRS}) 28 | get_filename_component(parent_dir_name ${dir} NAME) 29 | if(${parent_dir_name} STREQUAL thrift) 30 | list(REMOVE_ITEM XSENSSUITCONTROL_INCLUDE_DIRS ${dir}) 31 | get_filename_component(parent_dir_path ${dir} DIRECTORY) 32 | list(APPEND XSENSSUITCONTROL_INCLUDE_DIRS ${parent_dir_path}) 33 | endif() 34 | endforeach() 35 | 36 | # Setup the include directories 37 | target_include_directories(XsensSuitControl PUBLIC 38 | $) 39 | 40 | install(TARGETS XsensSuitControl 41 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} 42 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 43 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}) 44 | -------------------------------------------------------------------------------- /conf/iFeelOpenXRCalibration.sh: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | usage() { 5 | cat << EOF 6 | ************************************************************************************************************************************ 7 | Script for calibrating OpenVR world and iFeel world with the virtualizer world. 8 | The script should be launched expecting the subject inside the virtualizer in T-Pose. 9 | 10 | OPTION: 11 | 12 | EXAMPLE USAGE: ./iFeelOpenXRCalibration.sh 0.5 13 | ************************************************************************************************************************************ 14 | 15 | EOF 16 | } 17 | 18 | 19 | ################################################################################ 20 | # "MAIN" FUNCTION: # 21 | ################################################################################ 22 | usage 23 | if [ $# -lt 1 ]; then 24 | HEIGHT_CORRECTION_M=-0.1 25 | echo "[WARNING] No option passed, using default HEIGHT_CORRECTION_M "$HEIGHT_CORRECTION_M 26 | echo "" 27 | else 28 | HEIGHT_CORRECTION_M=$1 29 | fi 30 | 31 | # echo "OpenVR: resetting seated position" 32 | # echo "resetSeatedPosition" | yarp rpc /OpenVRTrackersModule/rpc 33 | 34 | echo "OpenXR: resetting seated position" 35 | echo "alignRootFrameToHeadset" | yarp rpc /joypadDevice/Oculus/rpc 36 | 37 | echo "Creating static transform: vive_tracker_waist_pose -> world" 38 | echo "set_static_transform_rad vive_tracker_waist_pose world 0.0 $HEIGHT_CORRECTION_M 0.1 0.0 -1.5708 -1.5708" | yarp rpc /transformServer/rpc 39 | echo "Creating static transform: world -> root_link_desired" 40 | echo "set_static_transform_rad world root_link_desired 0.0 0 0 0.0 -0.2 0.0" | yarp rpc /transformServer/rpc 41 | 42 | exit 0 43 | -------------------------------------------------------------------------------- /clients/HumanWrench_nwc_yarp/HumanWrench_nwc_yarp.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef HDE_DEVICES_HUMANWRENCH_NWC_YARP 5 | #define HDE_DEVICES_HUMANWRENCH_NWC_YARP 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | namespace trintrin::msgs { 17 | class HumanWrench; 18 | } // namespace trintrin::msgs 19 | namespace hde::devices { 20 | class HumanWrench_nwc_yarp; 21 | } // namespace hde::devices 22 | 23 | class hde::devices::HumanWrench_nwc_yarp final 24 | : public yarp::dev::DeviceDriver 25 | // inherite from the interface to be exposed 26 | , public hde::interfaces::IHumanWrench 27 | // implement the callback to read the thrifted message 28 | , public yarp::os::TypedReaderCallback 29 | // implement the periodic thread 30 | , public yarp::os::PeriodicThread 31 | { 32 | private: 33 | class impl; 34 | std::unique_ptr pImpl; 35 | 36 | public: 37 | HumanWrench_nwc_yarp(); 38 | ~HumanWrench_nwc_yarp() override; 39 | 40 | 41 | 42 | // DeviceDriver interface 43 | bool open(yarp::os::Searchable& config) override; 44 | bool close() override; 45 | 46 | // PeriodicThread 47 | void run() override; 48 | void threadRelease() override; 49 | 50 | // TypedReaderCallback 51 | void onRead(trintrin::msgs::HumanWrench& humanWrench) override; 52 | 53 | // IHumanWrench interface 54 | std::vector getWrenchSourceNames() const override; 55 | 56 | size_t getNumberOfWrenchSources() const override; 57 | 58 | std::vector getWrenches() const override; 59 | }; 60 | 61 | #endif // HDE_DEVICES_HUMANWRENCH_NWC_YARP 62 | 63 | -------------------------------------------------------------------------------- /app/xml/applications/WearableDevices-Dumper.xml: -------------------------------------------------------------------------------- 1 | 2 | WearableDevices-Dumper 3 | An application for dumping wearable data 4 | 5 | 6 | yarpdatadumper 7 | --name /wearableData/FTshoes/left --type bottle --txTime --rxTime 8 | ${generic_node} 9 | yarpdatadumper 10 | 11 | 12 | 13 | yarpdatadumper 14 | --name /wearableData/FTshoes/right --type bottle --txTime --rxTime 15 | ${generic_node} 16 | yarpdatadumper 17 | 18 | 19 | 20 | yarpdatadumper 21 | --name /wearableData/xsens --type bottle --txTime --rxTime 22 | ${generic_node} 23 | yarpdatadumper 24 | 25 | 26 | 27 | yarpdatadumper 28 | --name /wearableData/SkinInsoles --type bottle --txTime --rxTime 29 | ${generic_node} 30 | yarpdatadumper 31 | 32 | 33 | 34 | /FTShoeLeft/WearableData/data:o 35 | /wearableData/FTshoes/left 36 | udp 37 | 38 | 39 | 40 | /FTShoeRight/WearableData/data:o 41 | /wearableData/FTshoes/right 42 | udp 43 | 44 | 45 | 46 | /XSensSuit/WearableData/data:o 47 | /wearableData/xsens 48 | udp 49 | 50 | 51 | 52 | /SkinInsoles/WearableData/data:o 53 | /wearableData/SkinInsoles 54 | udp 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /clients/HumanDynamics_nwc_yarp/HumanDynamics_nwc_yarp.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef HDE_DEVICES_HUMANDYNAMICS_NEC_YARP 5 | #define HDE_DEVICES_HUMANDYNAMICS_NEC_YARP 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | namespace trintrin::msgs { 17 | class HumanDynamics; 18 | } // namespace trintrin::msgs 19 | namespace hde::devices { 20 | class HumanDynamics_nwc_yarp; 21 | } // namespace hde::devices 22 | 23 | class hde::devices::HumanDynamics_nwc_yarp final 24 | : public yarp::dev::DeviceDriver 25 | // inherite from the interface to be exposed 26 | , public hde::interfaces::IHumanDynamics 27 | // implement the callback to read the thrifted message 28 | , public yarp::os::TypedReaderCallback 29 | // implement the periodic thread 30 | , public yarp::os::PeriodicThread 31 | { 32 | private: 33 | class impl; 34 | std::unique_ptr pImpl; 35 | 36 | public: 37 | HumanDynamics_nwc_yarp(); 38 | ~HumanDynamics_nwc_yarp() override; 39 | 40 | 41 | 42 | // DeviceDriver interface 43 | bool open(yarp::os::Searchable& config) override; 44 | bool close() override; 45 | 46 | // PeriodicThread 47 | void run() override; 48 | void threadRelease() override; 49 | 50 | // TypedReaderCallback 51 | void onRead(trintrin::msgs::HumanDynamics& humanDynamics) override; 52 | 53 | // IHumanDynamics interface 54 | std::vector getJointNames() const override; 55 | 56 | size_t getNumberOfJoints() const override; 57 | 58 | std::vector getJointTorques() const override; 59 | }; 60 | 61 | #endif // HDE_DEVICES_HUMANDYNAMICS_NEC_YARP 62 | 63 | -------------------------------------------------------------------------------- /conf/xml/RobotPositionController_iCub2_5.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | /iCub/RobotStateServer/state:o 8 | 9 | 10 | 11 | 12 | 0.100 13 | positionDirect 14 | 15.0 15 | 0.01 16 | 0.25 17 | 2.5 18 | 5000 19 | (head torso left_arm right_arm) 20 | /icub 21 | /robotPositionController 22 | (neck_pitch neck_roll neck_yaw) 23 | (torso_yaw torso_roll torso_pitch) 24 | (l_shoulder_pitch l_shoulder_roll l_shoulder_yaw l_elbow l_wrist_prosup l_wrist_pitch l_wrist_yaw) 25 | (r_shoulder_pitch r_shoulder_roll r_shoulder_yaw r_elbow r_wrist_prosup r_wrist_pitch r_wrist_yaw) 26 | 27 | 28 | RobotStateRemapper 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /conf/xml/HumanLoggerExample.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | /HDE/HumanStateServer/state:o 9 | 10 | 11 | 12 | /HDE/HumanDynamicsServer/torques:o 13 | 14 | 15 | 16 | 0.01 17 | matlab 18 | true 19 | true 20 | 21 | false 22 | test_human_logger 23 | 24 | 25 | 100000 26 | true 27 | 120.0 28 | 300 29 | true 30 | 31 | 32 | 33 | 34 | HumanState_nwc_yarp 35 | HumanDynamics_nwc_yarp 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /app/xml/applications/WearableDevices.xml: -------------------------------------------------------------------------------- 1 | 2 | WearableDevices 3 | An application for running wearable devices 4 | 5 | 6 | 7 | yarprobotinterface 8 | --config XsensSuitWearableDevice.xml 9 | YARP_FORWARD_LOG_ENABLE=1 10 | Run xsens suit wearable device 11 | localhost 12 | 13 | 14 | 15 | 16 | yarprobotinterface 17 | --config FTShoeLeftWearableDevice.xml 18 | YARP_FORWARD_LOG_ENABLE=1 19 | Run left ftshoe wearable device 20 | localhost 21 | 22 | 23 | 24 | 25 | yarprobotinterface 26 | --config FTShoeRightWearableDevice.xml 27 | YARP_FORWARD_LOG_ENABLE=1 28 | Run right ftshoe wearable device 29 | localhost 30 | 31 | 32 | 33 | 34 | yarprobotinterface 35 | --config SkinInsolesWearableDevice.xml 36 | YARP_FORWARD_LOG_ENABLE=1 37 | Run skin insoles wearable device 38 | localhost 39 | 40 | 41 | 42 | 43 | yarprobotinterface 44 | --config ICubWearableDevice.xml 45 | YARP_FORWARD_LOG_ENABLE=1 46 | Run icub wearable device 47 | localhost 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /interfaces/IWear/include/Wearable/IWear/Sensors/IOrientationSensor.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef WEARABLE_IORIENTATION_SENSOR_H 5 | #define WEARABLE_IORIENTATION_SENSOR_H 6 | 7 | #include "Wearable/IWear/Sensors/ISensor.h" 8 | #include "Wearable/IWear/Utils.h" 9 | 10 | namespace wearable { 11 | namespace sensor { 12 | class IOrientationSensor; 13 | } // namespace sensor 14 | } // namespace wearable 15 | 16 | class wearable::sensor::IOrientationSensor : public wearable::sensor::ISensor 17 | { 18 | public: 19 | IOrientationSensor(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) 20 | : ISensor(aName, aStatus) 21 | { 22 | m_type = SensorType::OrientationSensor; 23 | } 24 | 25 | virtual ~IOrientationSensor() = default; 26 | virtual bool getOrientationAsQuaternion(Quaternion& orientation) const = 0; 27 | 28 | inline bool getOrientationAsRPY(Vector3& orientation) const; 29 | inline bool getOrientationAsRotationMatrix(Matrix3& orientation) const; 30 | 31 | inline static const std::string getPrefix(); 32 | }; 33 | 34 | inline const std::string wearable::sensor::IOrientationSensor::getPrefix() 35 | { 36 | return "orient" + wearable::Separator; 37 | } 38 | 39 | inline bool wearable::sensor::IOrientationSensor::getOrientationAsRPY(Vector3& orientation) const 40 | { 41 | Quaternion quat; 42 | if (!getOrientationAsQuaternion(quat)) { 43 | return false; 44 | } 45 | orientation = utils::quaternionToRPY(quat); 46 | return true; 47 | }; 48 | 49 | inline bool 50 | wearable::sensor::IOrientationSensor::getOrientationAsRotationMatrix(Matrix3& orientation) const 51 | { 52 | Quaternion quat; 53 | if (!getOrientationAsQuaternion(quat)) { 54 | return false; 55 | } 56 | orientation = utils::quaternionToRotationMatrix(quat); 57 | return true; 58 | }; 59 | 60 | #endif // WEARABLE_IORIENTATION_SENSOR_H 61 | -------------------------------------------------------------------------------- /conf/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | set (HDE_PROJECT_NAME HumanDynamicsEstimation) 5 | 6 | # HDE xml files installation 7 | file (GLOB HDE_XML_FILES xml/*.xml) 8 | 9 | install(FILES ${HDE_XML_FILES} 10 | DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${HDE_PROJECT_NAME}/) 11 | 12 | # HDE ini files installation 13 | file (GLOB HDE_INI_FILES app/*.ini) 14 | 15 | install(FILES ${HDE_INI_FILES} 16 | DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${HDE_PROJECT_NAME}/) 17 | 18 | # Install yarpmanager application files 19 | file (GLOB HDE_APP_FILES xml/applications/*.xml) 20 | 21 | yarp_install(FILES ${HDE_APP_FILES} DESTINATION ${YARP_APPLICATIONS_INSTALL_DIR}) 22 | 23 | # Install robot urdf files 24 | file (GLOB ROBOT_URDF_FILES urdfs/*.urdf) 25 | yarp_install(FILES ${ROBOT_URDF_FILES} 26 | DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${HDE_PROJECT_NAME}/) 27 | 28 | # Install custom constraint matrix 29 | file(GLOB scripts config/*.ini) 30 | yarp_install(FILES ${scripts} DESTINATION ${YARP_CONTEXTS_INSTALL_DIR}) 31 | 32 | 33 | # ROS related installation 34 | set (ROS_PROJECT_NAME HDERviz) 35 | file (GLOB ROS_PACKAGE_FILE ros/package.xml) 36 | file (GLOB ROS_LAUNCH_FILES ros/launch/*.launch) 37 | file (GLOB ROS_RVIZ_FILES ros/rviz/*.rviz) 38 | 39 | # Copy the package.xml, which is necessary to be treated as 40 | # a ROS package 41 | install(FILES ${ROS_PACKAGE_FILE} 42 | DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${ROS_PROJECT_NAME}/) 43 | 44 | 45 | # Install the .launch file and the config file 46 | install(FILES ${ROS_LAUNCH_FILES} 47 | DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${ROS_PROJECT_NAME}/launch/) 48 | 49 | install(FILES ${ROS_RVIZ_FILES} 50 | DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${ROS_PROJECT_NAME}/rviz/) 51 | 52 | # Install robot urdf files 53 | install(FILES ${ROBOT_URDF_FILES} 54 | DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${ROS_PROJECT_NAME}/urdfs/) 55 | -------------------------------------------------------------------------------- /wrappers/IWearActuators/include/IWearActuatorsWrapper.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef IWEACTUATORSWRAPPERS_H 5 | #define IWEACTUATORSWRAPPERS_H 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "trintrin/msgs/WearableActuatorCommand.h" 15 | #include "trintrin/msgs/GloveActuatorCommand.h" 16 | 17 | namespace wearable { 18 | namespace wrappers { 19 | class IWearActuatorsWrapper; 20 | } 21 | } // namespace wearable 22 | 23 | class wearable::wrappers::IWearActuatorsWrapper 24 | : public yarp::dev::DeviceDriver 25 | , public yarp::dev::IWrapper 26 | , public yarp::dev::IMultipleWrapper 27 | , public yarp::os::PeriodicThread 28 | , public yarp::os::TypedReaderCallback 29 | , public yarp::os::TypedReaderCallback 30 | { 31 | private: 32 | class impl; 33 | std::unique_ptr pImpl; 34 | 35 | public: 36 | IWearActuatorsWrapper(); 37 | ~IWearActuatorsWrapper() override; 38 | 39 | // DeviceDriver interface 40 | bool open(yarp::os::Searchable& config) override; 41 | bool close() override; 42 | 43 | // TypedReaderCallback 44 | void onRead(trintrin::msgs::WearableActuatorCommand& wearableActuatorCommand) override; 45 | void onRead(trintrin::msgs::GloveActuatorCommand& gloveActuatorCommand) override; 46 | 47 | // PeriodicThread 48 | void run() override; 49 | void threadRelease() override; 50 | 51 | // IWrapper interface 52 | bool attach(yarp::dev::PolyDriver* poly) override; 53 | bool detach() override; 54 | 55 | // IMultipleWrapper interface 56 | bool attachAll(const yarp::dev::PolyDriverList& driverList) override; 57 | bool detachAll() override; 58 | }; 59 | 60 | #endif // IWEACTUATORSWRAPPERS_H 61 | -------------------------------------------------------------------------------- /XSensMVN/include/XSensLogger.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef XSENS_LOGGER_H 5 | #define XSENS_LOGGER_H 6 | 7 | /* Hack required to print verbose log messages when not in Release 8 | * It is not possible to directly use the NDEBUG variable since XSens defines regardless from the 9 | * compile option 10 | * This brings as consequence that the Logger should be included in the .h files ALWAYS before XSens 11 | * SDK files 12 | */ 13 | #ifndef NDEBUG 14 | #define XS_VERBOSE_LOG 15 | #endif 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | // Define macros to print YARP-like info, warning, and error messages with exactly the same usage 22 | // of std::cout (operator <<) 23 | #ifndef xsError 24 | #define xsError \ 25 | std::cerr << std::endl \ 26 | << printLogMessage("[ERROR]", __FILE__, std::to_string(__LINE__), __FUNCTION__) 27 | #endif 28 | 29 | #ifndef xsWarning 30 | #define xsWarning \ 31 | std::cout << std::endl \ 32 | << printLogMessage("[WARNING]", __FILE__, std::to_string(__LINE__), __FUNCTION__) 33 | #endif 34 | 35 | #ifndef xsInfo 36 | #define xsInfo \ 37 | std::cout << std::endl \ 38 | << printLogMessage("[INFO]", __FILE__, std::to_string(__LINE__), __FUNCTION__) 39 | #endif 40 | 41 | // Utility function to print "advanced" log messages 42 | inline std::string printLogMessage(const std::string& msgType, 43 | const std::string& file = "", 44 | const std::string& line = "", 45 | const std::string& function = "") 46 | { 47 | std::string filename = file.substr(file.find_last_of("/\\") + 1); 48 | std::string ss; 49 | ss = msgType + " "; 50 | 51 | // If debug print addictional metadata 52 | #ifdef XS_VERBOSE_LOG 53 | ss = ss + filename + "@" + function + ":" + line + " - "; 54 | #endif 55 | return ss; 56 | }; 57 | 58 | #endif // XSENS_LOGGER_H 59 | -------------------------------------------------------------------------------- /XSensMVN/test/testCalibrator.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #include "XSensCalibrationQualities.h" 5 | #include "XSensMVNCalibrator.h" 6 | 7 | #include "xme.h" 8 | 9 | #include 10 | #include 11 | 12 | int main(int argc, const char* argv[]) 13 | { 14 | // Create a licence object 15 | XmeLicense lic; 16 | 17 | // Hardcoded path to the runtime dependencies 18 | std::string rundepsFolder = "C:/Program Files/Xsens/MVN SDK 2018.0.3/SDK Files/rundeps"; 19 | 20 | // Configure the paths to the runtime required folders 21 | xmeSetPaths(rundepsFolder.c_str(), "", "", true); 22 | 23 | try { 24 | XmeControl* ctl = XmeControl::construct(); 25 | 26 | // Connect to the suit 27 | xsInfo << "Starting scan"; 28 | ctl->setScanMode(true); 29 | 30 | while (!ctl->status().isConnected()) 31 | std::this_thread::sleep_for(std::chrono::microseconds(10)); 32 | 33 | ctl->setScanMode(false); 34 | 35 | xsInfo << "Suit connected."; 36 | 37 | // List the available calibration types 38 | XsStringArray calibTypes = ctl->calibrationLabelList(); 39 | xsInfo << "Calibration types:"; 40 | for (auto& type : calibTypes) { 41 | xsInfo << type; 42 | } 43 | 44 | // Create the calibrator 45 | xsensmvn::XSensMVNCalibrator calib(*ctl, xsensmvn::CalibrationQuality::ACCEPTABLE); 46 | 47 | // Perform the calibration 48 | calib.calibrateWithType(std::string("Npose")); 49 | 50 | // Disconnect from the XSens suit and wait for a positive feedback 51 | ctl->disconnectHardware(); 52 | while (ctl->status().isConnected()) 53 | std::this_thread::sleep_for(std::chrono::microseconds(10)); 54 | } 55 | catch (XsException& exception) { 56 | xsError << "An error occurred: " << exception.text(); 57 | xmeTerminate(); 58 | return 1; 59 | } 60 | 61 | // Clean up and exit 62 | xmeTerminate(); 63 | return EXIT_SUCCESS; 64 | } 65 | -------------------------------------------------------------------------------- /devices/HumanWrenchProvider/HumanWrenchProvider.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef HDE_DEVICES_HUMANWRENCHPROVIDER 5 | #define HDE_DEVICES_HUMANWRENCHPROVIDER 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | 16 | namespace hde { 17 | namespace devices { 18 | class HumanWrenchProvider; 19 | } // namespace devices 20 | } // namespace hde 21 | 22 | class hde::devices::HumanWrenchProvider final 23 | : public yarp::dev::DeviceDriver 24 | , public yarp::dev::IMultipleWrapper 25 | , private yarp::os::PeriodicThread 26 | , public yarp::dev::IAnalogSensor 27 | , public hde::interfaces::IHumanWrench 28 | { 29 | private: 30 | class Impl; 31 | std::unique_ptr pImpl; 32 | 33 | public: 34 | HumanWrenchProvider(); 35 | ~HumanWrenchProvider() override; 36 | 37 | // DeviceDriver interface 38 | bool open(yarp::os::Searchable& config) override; 39 | bool close() override; 40 | 41 | // PeriodicThread 42 | void run() override; 43 | void threadRelease() override; 44 | 45 | // IMultipleWrapper interface 46 | bool attachAll(const yarp::dev::PolyDriverList& driverList) override; 47 | bool detachAll() override; 48 | 49 | // IAnalogSensor interface 50 | int read(yarp::sig::Vector& out) override; 51 | int getState(int ch) override; 52 | int getChannels() override; 53 | int calibrateSensor() override; 54 | int calibrateSensor(const yarp::sig::Vector& value) override; 55 | int calibrateChannel(int ch) override; 56 | int calibrateChannel(int ch, double value) override; 57 | 58 | // IHumanWrench 59 | std::vector getWrenchSourceNames() const override; 60 | size_t getNumberOfWrenchSources() const override; 61 | std::vector getWrenches() const override; 62 | }; 63 | 64 | #endif // HDE_DEVICES_HUMANWRENCHPROVIDER 65 | -------------------------------------------------------------------------------- /clients/HumanState_nwc_yarp/HumanState_nwc_yarp.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef HDE_DEVICES_HUMANSTATE_NWC_YARP 5 | #define HDE_DEVICES_HUMANSTATE_NWC_YARP 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | namespace trintrin::msgs { 17 | class HumanState; 18 | } 19 | namespace hde::devices { 20 | class HumanState_nwc_yarp; 21 | } // namespace hde::devices 22 | 23 | class hde::devices::HumanState_nwc_yarp final 24 | : public yarp::dev::DeviceDriver 25 | , public hde::interfaces::IHumanState 26 | , public yarp::os::TypedReaderCallback 27 | , public yarp::os::PeriodicThread 28 | { 29 | private: 30 | class impl; 31 | std::unique_ptr pImpl; 32 | 33 | public: 34 | HumanState_nwc_yarp(); 35 | ~HumanState_nwc_yarp() override; 36 | 37 | std::mutex mtx; 38 | 39 | // DeviceDriver interface 40 | bool open(yarp::os::Searchable& config) override; 41 | bool close() override; 42 | 43 | // PeriodicThread 44 | void run() override; 45 | void threadRelease() override; 46 | 47 | // TypedReaderCallback 48 | void onRead(trintrin::msgs::HumanState& humanState) override; 49 | 50 | // IHumanState interface 51 | std::vector getJointNames() const override; 52 | std::string getBaseName() const override; 53 | size_t getNumberOfJoints() const override; 54 | 55 | std::vector getJointPositions() const override; 56 | std::vector getJointVelocities() const override; 57 | 58 | std::array getBasePosition() const override; 59 | std::array getBaseOrientation() const override; 60 | 61 | std::array getBaseVelocity() const override; 62 | 63 | std::array getCoMPosition() const override; 64 | std::array getCoMVelocity() const override; 65 | }; 66 | 67 | #endif // HDE_DEVICES_HUMANSTATE_NWC_YARP 68 | -------------------------------------------------------------------------------- /doc/How-to-run-FTshoes.md: -------------------------------------------------------------------------------- 1 | # How to run FTShoes as Wearable Device 2 | 3 | This page describes how to run the [FTShoes](https://github.com/robotology/forcetorque-yarp-devices/tree/master/ftShoe) as a wearable device source. Further information on how to install and use the shoes can be found at https://github.com/robotology/forcetorque-yarp-devices/tree/master/ftShoe. 4 | 5 | The configuration file to run the FTShoes as wearable device is [`FTShoesWearableDevice.xml`](https://github.com/robotology/wearables/blob/master/app/xml/FTShoesWearableDevice.xml), and it can be launched with: 6 | ``` 7 | yarprobotinterface --config FTShoesWearableDevice.xml 8 | ``` 9 | The two shoes can also be launched separately using [`FTShoeLeftWearableDevice.xml`](https://github.com/robotology/wearables/blob/master/app/xml/FTShoeLeftWearableDevice.xml) and [`FTShoeRightWearableDevice.xml`](https://github.com/robotology/wearables/blob/master/app/xml/FTShoeRightWearableDevice.xml). 10 | 11 | ### Before running 12 | Before running the device make sure that: 13 | - [`yarpserver`](https://www.yarp.it/yarpserver.html) is running 14 | - The shoes are connected and recognized by the laptop 15 | - The can address of the FT sensors in the shoes is the same of the configuration file (e.g. `ftShoe_Left_Front` -> ` 0x01 `) 16 | - There is not another device running that is using the port names. In case multiple pairs of shoes have to run simultaneously, it is necessary to differentiate the name of the ports like in [`FTShoesWearableDevice_2.xml`](https://github.com/robotology/wearables/blob/master/app/xml/FTShoesWearableDevice_2.xml). 17 | - The `inSituMatrices` contained in the configuration file correspond to the shoes you are using. 18 | 19 | 20 | #### Calibration 21 | The FT data streamed by the shoes may be characterized by an offset. The procedure for removing the offset is described at https://github.com/robotology/forcetorque-yarp-devices/tree/master/ftShoe#how-to-use-the-ftshoes. 22 | 23 | #### Data 24 | If the device is running correctly, the stream of wearable data can be read with: 25 | ``` 26 | yarp read ... /FTShoeLeft/WearableData/data:o 27 | yarp read ... /FTShoeRight/WearableData/data:o 28 | ``` 29 | -------------------------------------------------------------------------------- /conf/xml/RobotPositionController_iCub.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | /iCub/RobotStateServer/state:o 8 | 9 | 10 | 11 | 12 | 0.100 13 | positionDirect 14 | 15.0 15 | 0.01 16 | 0.25 17 | 2.5 18 | 5000 19 | (head torso left_arm right_arm left_leg right_leg) 20 | /icub 21 | /robotPositionController 22 | (neck_pitch neck_roll neck_yaw) 23 | (torso_yaw torso_roll torso_pitch) 24 | (l_shoulder_pitch l_shoulder_roll l_shoulder_yaw l_elbow l_wrist_prosup l_wrist_pitch l_wrist_yaw) 25 | (r_shoulder_pitch r_shoulder_roll r_shoulder_yaw r_elbow r_wrist_prosup r_wrist_pitch r_wrist_yaw) 26 | (l_hip_pitch l_hip_roll l_hip_yaw l_knee l_ankle_pitch l_ankle_roll) 27 | (r_hip_pitch r_hip_roll r_hip_yaw r_knee r_ankle_pitch r_ankle_roll) 28 | 29 | 30 | RobotStateRemapper 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /interfaces/IWear/include/Wearable/IWear/Sensors/IForceTorque6DSensor.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef WEARABLE_IFORCE_TORQUE_6D_SENSOR_H 5 | #define WEARABLE_IFORCE_TORQUE_6D_SENSOR_H 6 | 7 | #include "Wearable/IWear/Sensors/ISensor.h" 8 | 9 | namespace wearable { 10 | namespace sensor { 11 | class IForceTorque6DSensor; 12 | } // namespace sensor 13 | } // namespace wearable 14 | 15 | class wearable::sensor::IForceTorque6DSensor : public wearable::sensor::ISensor 16 | { 17 | public: 18 | IForceTorque6DSensor(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) 19 | : ISensor(aName, aStatus) 20 | { 21 | m_type = SensorType::ForceTorque6DSensor; 22 | } 23 | 24 | virtual ~IForceTorque6DSensor() = default; 25 | 26 | virtual bool getForceTorque6D(Vector3& force3D, Vector3& torque3D) const = 0; 27 | 28 | inline bool getForceTorque6D(Vector6& forceTorque6D) const; 29 | inline bool getForceTorque3DForce(Vector3& force3D) const; 30 | inline bool getForceTorque3DTorque(Vector3& torque3D) const; 31 | 32 | inline static const std::string getPrefix(); 33 | }; 34 | 35 | inline const std::string wearable::sensor::IForceTorque6DSensor::getPrefix() 36 | { 37 | return "ft6D" + wearable::Separator; 38 | } 39 | bool wearable::sensor::IForceTorque6DSensor::getForceTorque6D( 40 | wearable::Vector6& forceTorque6D) const 41 | { 42 | wearable::Vector3 force3D, torque3D; 43 | const bool ok = getForceTorque6D(force3D, torque3D); 44 | forceTorque6D = {force3D[0], force3D[1], force3D[2], torque3D[0], torque3D[1], torque3D[2]}; 45 | return ok; 46 | } 47 | 48 | bool wearable::sensor::IForceTorque6DSensor::getForceTorque3DForce(wearable::Vector3& force3D) const 49 | { 50 | wearable::Vector3 dummy; 51 | return getForceTorque6D(force3D, dummy); 52 | } 53 | 54 | bool wearable::sensor::IForceTorque6DSensor::getForceTorque3DTorque( 55 | wearable::Vector3& torque3D) const 56 | { 57 | wearable::Vector3 dummy; 58 | return getForceTorque6D(dummy, torque3D); 59 | } 60 | 61 | #endif // WEARABLE_IFORCE_TORQUE_6D_SENSOR_H 62 | -------------------------------------------------------------------------------- /interfaces/IWear/include/Wearable/IWear/Actuators/IActuator.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef WEARABLE_IACTUATOR_H 5 | #define WEARABLE_IACTUATOR_H 6 | 7 | #include "Wearable/IWear/Common.h" 8 | 9 | #include 10 | 11 | namespace wearable { 12 | 13 | namespace actuator { 14 | 15 | using ActuatorName = std::string; 16 | 17 | enum class ActuatorType 18 | { 19 | Haptic = 0, 20 | Motor, 21 | Heater, 22 | Invalid 23 | }; 24 | 25 | enum class ActuatorStatus 26 | { 27 | Error = 0, 28 | Ok, 29 | Unknown, 30 | }; 31 | 32 | class IActuator; 33 | } // namespace actuator 34 | } // namespace wearable 35 | 36 | class wearable::actuator::IActuator : public wearable::IWearableDevice 37 | { 38 | protected: 39 | ActuatorName m_name; 40 | ActuatorType m_type; 41 | std::atomic m_status; 42 | 43 | public: 44 | IActuator(ActuatorName aName = {}, ActuatorStatus aStatus = ActuatorStatus::Unknown) 45 | : m_name{aName} 46 | , m_status{aStatus} 47 | { 48 | m_wearable_element_type = ElementType::WearableActuator; 49 | } 50 | 51 | virtual ~IActuator() = default; 52 | 53 | inline ElementType getWearableElementType() const; 54 | 55 | inline ActuatorName getActuatorName() const; 56 | inline ActuatorType getActuatorType() const; 57 | inline ActuatorStatus getActuatorStatus() const; 58 | }; 59 | 60 | inline wearable::ElementType wearable::actuator::IActuator::getWearableElementType() const 61 | { 62 | return m_wearable_element_type; 63 | } 64 | 65 | inline wearable::actuator::ActuatorName wearable::actuator::IActuator::getActuatorName() const 66 | { 67 | return m_name; 68 | } 69 | 70 | inline wearable::actuator::ActuatorType wearable::actuator::IActuator::getActuatorType() const 71 | { 72 | return m_type; 73 | } 74 | 75 | inline wearable::actuator::ActuatorStatus wearable::actuator::IActuator::getActuatorStatus() const 76 | { 77 | return m_status; 78 | } 79 | 80 | #endif // WEARABLE_IACTUATOR_H 81 | -------------------------------------------------------------------------------- /conf/xml/transformServer_to_iwear.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0.01 7 | ftc_yarp_only.xml 8 | /IFrameTransformToIWear/tf 9 | /IFrameTransformToIWear/tf/local_rpc 10 | 11 | 12 | 13 | 14 | TransformServer 15 | root_link_desired 16 | PoseSensor 17 | (root_link_desired 18 | openxr_origin 19 | openxr_head 20 | stable_waist 21 | vive_tracker_waist_pose 22 | vive_tracker_right_elbow_pose 23 | vive_tracker_left_elbow_pose 24 | vive_tracker_left_foot_pose 25 | vive_tracker_right_foot_pose) 26 | 27 | 28 | TransformClient 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 0.01 38 | /Wearable/OpenXRTransform/data:o 39 | /Wearable/OpenXRTransform/metadata:o 40 | 41 | 42 | 43 | IFrameTransformToIWear 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /cmake/AddWarningsConfigurationToTarget.cmake: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | include(CMakeParseArguments) 5 | 6 | function(ADD_WARNINGS_CONFIGURATION_TO_TARGET) 7 | 8 | set(_options PRIVATE 9 | PUBLIC 10 | INTERFACE) 11 | set(_oneValueArgs ) 12 | set(_multiValueArgs TARGETS) 13 | 14 | cmake_parse_arguments(_AWC2T "${_options}" 15 | "${_oneValueArgs}" 16 | "${_multiValueArgs}" 17 | "${ARGN}") 18 | 19 | # Check the kind of visibility requested 20 | set(VISIBILITIES) 21 | if (${_AWC2T_PRIVATE}) 22 | list(APPEND VISIBILITIES PRIVATE) 23 | endif() 24 | if (${_AWC2T_PUBLIC}) 25 | list(APPEND VISIBILITIES PUBLIC) 26 | endif() 27 | if (${_AWC2T_INTERFACE}) 28 | list(APPEND VISIBILITIES INTERFACE) 29 | endif() 30 | 31 | 32 | foreach(TARGET ${_AWC2T_TARGETS}) 33 | foreach(VISIBILITY ${VISIBILITIES}) 34 | #setting debug options 35 | set(COMPILE_OPTIONS "") 36 | if(MSVC) 37 | ### 38 | list(APPEND COMPILE_OPTIONS "/W3") 39 | else() 40 | ##Other systems 41 | if(${CMAKE_CXX_COMPILER_ID} MATCHES "Clang") 42 | list(APPEND COMPILE_OPTIONS "-Weverything") 43 | list(APPEND COMPILE_OPTIONS "-pedantic") 44 | list(APPEND COMPILE_OPTIONS "-Wnon-virtual-dtor") 45 | list(APPEND COMPILE_OPTIONS "-Woverloaded-virtual") 46 | list(APPEND COMPILE_OPTIONS "-Wc++11-extensions") 47 | #Remove some other warnings on paddings 48 | list(APPEND COMPILE_OPTIONS "-Wno-padded") 49 | list(APPEND COMPILE_OPTIONS "-Wno-cast-align") 50 | list(APPEND COMPILE_OPTIONS "-Wno-c++98-compat") 51 | 52 | elseif(${CMAKE_COMPILER_IS_GNUCC}) 53 | list(APPEND COMPILE_OPTIONS "-Wall") 54 | list(APPEND COMPILE_OPTIONS "-pedantic") 55 | list(APPEND COMPILE_OPTIONS "-Wextra") 56 | list(APPEND COMPILE_OPTIONS "-Woverloaded-virtual") 57 | endif() 58 | endif() 59 | 60 | target_compile_options(${TARGET} ${VISIBILITY} "$<$:${COMPILE_OPTIONS}>") 61 | 62 | endforeach() 63 | endforeach() 64 | 65 | endfunction() 66 | 67 | -------------------------------------------------------------------------------- /conf/xml/applications/HumanDynamicsEstimation-HumanDynamics.xml: -------------------------------------------------------------------------------- 1 | 2 | HumanDynamicsEstimation-HumanDynamics 3 | An application for running Human Dynamics Estimation (HDE) for only human 4 | 5 | 6 | 7 | yarprobotinterface 8 | --config Human.xml 9 | 10 | /XSensSuit/WearableData/data:o 11 | /FTShoeLeft/WearableData/data:o 12 | /FTShoeRight/WearableData/data:o 13 | 14 | YARP_FORWARD_LOG_ENABLE=1 15 | Run HDE for only human 16 | localhost 17 | 18 | 19 | 20 | 21 | HumanStateVisualizer 22 | --from HumanStateVisualizerWithDynamics.ini 23 | 24 | /HDE/HumanStateServer/state:o 25 | 26 | YARP_FORWARD_LOG_ENABLE=1 27 | Run the iDynTree Human Visualizer 28 | localhost 29 | 30 | 31 | 32 | 33 | yarprobotstatepublisher 34 | --period 0.0001 --name-prefix Human --tf-prefix /Human/ --model humanSubject01_66dof.urdf --reduced-model true --base-frame Pelvis --jointstates-topic "/Human/joint_states" 35 | 36 | /HumanDynamicsPublisher 37 | /Human/joint_states+@/HumanStatePublisher 38 | /tf+@/HumanStatePublisher 39 | 40 | YARP_FORWARD_LOG_ENABLE=1 41 | Run yarprobotstatepublisher with correct human model 42 | localhost 43 | 44 | 45 | 46 | 47 | roslaunch 48 | HDERviz HDERviz.launch 49 | YARP_FORWARD_LOG_ENABLE=1 50 | Run rviz 51 | localhost 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /devices/HumanStateProvider/HumanStateProvider.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef HDE_DEVICES_HUMANSTATEPROVIDER 5 | #define HDE_DEVICES_HUMANSTATEPROVIDER 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | namespace hde { 16 | namespace devices { 17 | class HumanStateProvider; 18 | } // namespace devices 19 | } // namespace hde 20 | 21 | class hde::devices::HumanStateProvider final 22 | : public yarp::dev::DeviceDriver 23 | , public yarp::dev::IMultipleWrapper 24 | , private yarp::os::PeriodicThread 25 | , public hde::interfaces::IHumanState 26 | , public hde::interfaces::IWearableTargets 27 | { 28 | private: 29 | class impl; 30 | std::unique_ptr pImpl; 31 | 32 | public: 33 | HumanStateProvider(); 34 | ~HumanStateProvider() override; 35 | 36 | // DeviceDriver interface 37 | bool open(yarp::os::Searchable& config) override; 38 | bool close() override; 39 | 40 | // PeriodicThread 41 | void run() override; 42 | void threadRelease() override; 43 | 44 | // IMultipleWrapper interface 45 | bool attachAll(const yarp::dev::PolyDriverList& driverList) override; 46 | bool detachAll() override; 47 | 48 | // IHumanState 49 | std::vector getJointNames() const override; 50 | size_t getNumberOfJoints() const override; 51 | std::string getBaseName() const override; 52 | std::vector getJointPositions() const override; 53 | std::vector getJointVelocities() const override; 54 | std::array getBasePosition() const override; 55 | std::array getBaseOrientation() const override; 56 | std::array getBaseVelocity() const override; 57 | std::array getCoMPosition() const override; 58 | std::array getCoMVelocity() const override; 59 | 60 | // IWearableTargets 61 | std::vector getAllTargetsName() const override; 62 | std::shared_ptr getTarget(const hde::TargetName name) const override; 63 | }; 64 | 65 | #endif // HDE_DEVICES_HUMANSTATEPROVIDER 66 | -------------------------------------------------------------------------------- /interfaces/IWear/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | 5 | cmake_minimum_required(VERSION 3.16) 6 | project(IWear LANGUAGES CXX VERSION ${PROJECT_VERSION}) 7 | 8 | include(GNUInstallDirs) 9 | 10 | # =============== 11 | # IWEAR INTERFACE 12 | # =============== 13 | 14 | # Policy to suppress An interface source of target "IWear" has a relative path warning 15 | # Policy CMP0076 is not set: target_sources() command converts relative pathsto absolute. 16 | cmake_policy(SET CMP0076 OLD) 17 | 18 | # Group files with the correct path for the target_sources command 19 | file(GLOB_RECURSE IWEAR_HEADERS_BUILD include/*.h) 20 | file(GLOB_RECURSE IWEAR_HEADERS_INSTALL 21 | RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} 22 | *.h) 23 | 24 | set(IWEAR_HEADERS 25 | $ 26 | $) 27 | 28 | add_library(${PROJECT_NAME} INTERFACE) 29 | add_library(IWear::IWear ALIAS IWear) 30 | target_sources(${PROJECT_NAME} INTERFACE ${IWEAR_HEADERS}) 31 | 32 | target_include_directories(${PROJECT_NAME} INTERFACE 33 | $ 34 | $) 35 | 36 | install( 37 | TARGETS ${PROJECT_NAME} 38 | EXPORT IWear) 39 | 40 | if(MSVC) 41 | # Import definitions from standard cmath 42 | target_compile_definitions(${PROJECT_NAME} INTERFACE "_USE_MATH_DEFINES") 43 | endif() 44 | 45 | # Install all the directories preserving their structure 46 | install( 47 | FILES include/Wearable/IWear/Common.h 48 | DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/Wearable/IWear) 49 | install( 50 | FILES include/Wearable/IWear/IWear.h 51 | DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/Wearable/IWear) 52 | install( 53 | FILES include/Wearable/IWear/Utils.h 54 | DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/Wearable/IWear) 55 | install( 56 | DIRECTORY include/Wearable/IWear/Sensors 57 | DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/Wearable/IWear) 58 | install( 59 | DIRECTORY include/Wearable/IWear/Actuators 60 | DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/Wearable/IWear) 61 | 62 | # Generate the PackageConfig.cmake file 63 | install_basic_package_files(IWear 64 | VERSION ${PROJECT_VERSION} 65 | COMPATIBILITY AnyNewerVersion 66 | EXPORT IWear 67 | NO_CHECK_REQUIRED_COMPONENTS_MACRO 68 | DEPENDENCIES YARP) 69 | -------------------------------------------------------------------------------- /devices/FTShoes/include/FTShoes.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | 5 | #ifndef WEAR_FTSHOES 6 | #define WEAR_FTSHOES 7 | 8 | #include "IWear/IWear.h" 9 | 10 | namespace wearable { 11 | namespace suit { 12 | class FTShoes; 13 | } 14 | } // namespace wearable 15 | 16 | class wearable::suit::FTShoes final : public wearable::IWear 17 | { 18 | private: 19 | class Impl; 20 | std::unique_ptr pImpl; 21 | 22 | public: 23 | FTShoes(); 24 | ~FTShoes() override; 25 | 26 | FTShoes(const FTShoes& other) = delete; 27 | FTShoes(FTShoes&& other) = delete; 28 | FTShoes& operator=(const FTShoes& other) = delete; 29 | FTShoes& operator=(FTShoes&& other) = delete; 30 | 31 | // ======= 32 | // GENERIC 33 | // ======= 34 | 35 | wearable::SensorPtr 36 | getSensor(const wearable::sensor::SensorName name) const override; 37 | 38 | wearable::VectorOfSensorPtr 39 | getSensors(const wearable::sensor::SensorType) const override; 40 | 41 | // ============== 42 | // SINGLE SENSORS 43 | // ============== 44 | 45 | virtual wearable::SensorPtr 46 | getAccelerometer(const wearable::sensor::SensorName name) const override; 47 | 48 | virtual wearable::SensorPtr 49 | getForce3DSensor(const wearable::sensor::SensorName name) const override; 50 | 51 | virtual wearable::SensorPtr 52 | getForceTorque6DSensor(const wearable::sensor::SensorName name) const override; 53 | 54 | virtual wearable::SensorPtr 55 | getGyroscope(const wearable::sensor::SensorName name) const override; 56 | 57 | virtual wearable::SensorPtr 58 | getMagnetometer(const wearable::sensor::SensorName name) const override; 59 | 60 | virtual wearable::SensorPtr 61 | getOrientationSensor(const wearable::sensor::SensorName name) const override; 62 | 63 | virtual wearable::SensorPtr 64 | getTemperatureSensor(const wearable::sensor::SensorName name) const override; 65 | 66 | virtual wearable::SensorPtr 67 | getTorque3DSensor(const wearable::sensor::SensorName name) const override; 68 | }; 69 | 70 | #endif // WEAR_FTSHOES 71 | -------------------------------------------------------------------------------- /conf/xml/applications/HumanDynamicsEstimation-HumanKinematics.xml: -------------------------------------------------------------------------------- 1 | 2 | HumanDynamicsEstimation-HumanKinematics 3 | An application for running Human Dynamics Estimation (HDE) for only human 4 | 5 | 6 | yarprobotinterface 7 | --config TransformServer.xml 8 | localhost 9 | 10 | 11 | 12 | 13 | yarprobotinterface 14 | --config HumanStateProvider.xml 15 | 16 | /transformServer/rpc 17 | /transformServer/transforms:o 18 | /XSensSuit/WearableData/data:o 19 | /FTShoeLeft/WearableData/data:o 20 | /FTShoeRight/WearableData/data:o 21 | 22 | YARP_FORWARD_LOG_ENABLE=1 23 | Run HDE for only human 24 | localhost 25 | 26 | 27 | 28 | 29 | HumanStateVisualizer 30 | --from HumanStateVisualizer.ini 31 | 32 | /HDE/HumanStateServer/state:o 33 | 34 | YARP_FORWARD_LOG_ENABLE=1 35 | Run the iDynTree Human Visualizer 36 | localhost 37 | 38 | 39 | 40 | 41 | yarprobotstatepublisher 42 | --period 0.0001 --name-prefix Human --tf-prefix /Human/ --model humanSubject01_66dof.urdf --reduced-model true --base-frame Pelvis --jointstates-topic "/Human/joint_states" 43 | 44 | /Human/joint_states+@/HumanStatePublisher 45 | /tf+@/HumanStatePublisher 46 | 47 | YARP_FORWARD_LOG_ENABLE=1 48 | Run yarprobotstatepublisher with correct human model 49 | localhost 50 | 51 | 52 | 53 | 54 | roslaunch 55 | HDERviz HDERviz.launch 56 | YARP_FORWARD_LOG_ENABLE=1 57 | Run rviz 58 | localhost 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /devices/Paexo/test-application/paexo-test-application.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | #define MIN_MOTOR_POSITION 30 14 | #define MAX_MOTOR_POSITION 50 15 | #define MOTOR_STEP_SIZE 2 //Could go as low as 0.2 16 | using namespace yarp::os; 17 | using namespace wearable; 18 | using YarpBufferedPort = yarp::os::BufferedPort; 19 | 20 | int main() { 21 | 22 | yarp::os::Network::init(); 23 | 24 | std::string inPort = "/Paexo/WearableActuatorsCommand/input:i"; 25 | std::string outPort = "/Paexo/WearableActuatorsCommand/output:o"; 26 | 27 | std::vector PaexoActuators = {"Paexo::motor::LeftMotor", "Paexo::motor::RightMotor"}; 28 | 29 | std::vector PaexoActuatorPortNames = {"/Paexo/motor/LeftMotor"}; 30 | std::vector> PaexoActuatorPorts; 31 | 32 | // Yarp buffered ports for motor control 33 | for (const auto& portName : PaexoActuatorPortNames) 34 | { 35 | std::unique_ptr port = std::make_unique(); 36 | 37 | if (!port->open(portName + ":o")) 38 | { 39 | yError() << "Failed to open port " << (portName + ":o"); 40 | return -1; 41 | } 42 | 43 | if (!yarp::os::Network::connect(portName + ":o", portName + ":i")) 44 | { 45 | yError() << "Failed to connect ports " << (portName + ":o") << " and " << (portName + ":i"); 46 | return -1; 47 | } 48 | 49 | PaexoActuatorPorts.push_back(std::move(port)); 50 | } 51 | 52 | 53 | while (true) { 54 | 55 | for (const auto& port : PaexoActuatorPorts) 56 | { 57 | double current_motor_position = MIN_MOTOR_POSITION; 58 | 59 | while (current_motor_position < MAX_MOTOR_POSITION) 60 | { 61 | yarp::os::Bottle& cmd = port->prepare(); 62 | 63 | cmd.clear(); 64 | 65 | yInfo() << "Sending motor position " << current_motor_position; 66 | cmd.addDouble(current_motor_position); 67 | 68 | port->write(); 69 | 70 | Time::delay(0.5); 71 | 72 | current_motor_position += MOTOR_STEP_SIZE; 73 | } 74 | } 75 | } 76 | 77 | return 0; 78 | } 79 | -------------------------------------------------------------------------------- /app/xml/iWearLoggerExampleTemplate.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 0.01 11 | 12 | 13 | 14 | (matlab, yarp) 15 | 16 | false 17 | false 18 | false 19 | false 20 | true 21 | true 22 | true 23 | false 24 | true 25 | false 26 | false 27 | false 28 | false 29 | false 30 | false 31 | false 32 | false 33 | 34 | true 35 | test_iwear_logger 36 | /path/to/some/folder/ 37 | 100000 38 | true 39 | 120.0 40 | 300 41 | true 42 | 43 | 44 | 45 | 46 | ProducerDevice 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /devices/XsensSuit/conf/XsensSuit.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | "C:\Program Files\Xsens\MVN SDK 2018.0.3\SDK Files\rundeps" 6 | 7 | 8 | 9 | 10 | FullBody 11 | 12 | 13 | singleLevel 14 | 15 | 16 | Npose 17 | 18 | 19 | acceptable 20 | 21 | 60 22 | 23 | 24 | 120 25 | 26 | 27 | 0.07 28 | 1.71 29 | 1.71 30 | 0.26 31 | 0.87 32 | 0.25 33 | 0.50 34 | 0.34 35 | 0.02 36 | 37 | 38 | 39 | false 40 | true 41 | true 42 | 43 | 44 | -------------------------------------------------------------------------------- /doc/How-to-run-iCub-as-wearable-source.md: -------------------------------------------------------------------------------- 1 | # How to run iCub as Wearable Device 2 | 3 | This page describes how to run the [`iCub`](http://www.icub.org/) humanoid robot as a wearable device source, by exposing its sensors measurement and joints state. 4 | 5 | 6 | The configuration file to run `iCub` as wearable device is [`ICubWearableDevice.xml`](https://github.com/robotology/wearables/blob/master/app/xml/ICubWearableDevice.xml), and it can be launched with: 7 | ``` 8 | yarprobotinterface --config ICubWearableDevice.xml 9 | ``` 10 | 11 | 12 | #### Before running 13 | Before running the device make sure that: 14 | - [`yarpserver`](https://www.yarp.it/yarpserver.html) is running 15 | - The `YARP_ROBOT_NAME` is set to the proper robot name. 16 | - The robot is currently running (either [Gazebo simulator](https://github.com/robotology/icub-gazebo) or real robot making sure it is runned with the [proper configuration for whole-body-dynamics](https://github.com/robotology/whole-body-controllers/blob/master/doc/How-to-setup-the-robot-for-wbc-experiments.md)) 17 | - If the robot is running in simulation, the `whole-body-dynamics` estimatior has to be launched 18 | ``` 19 | yarprobotinterface --config launch-wholebodydynamics.xml 20 | ``` 21 | When the real robot is running, `whole-body-dynamics` estimatior should be running if the robot is launched with the [proper configuration](https://github.com/robotology/whole-body-controllers/blob/master/doc/How-to-setup-the-robot-for-wbc-experiments.md). 22 | To verify if the estimator is running, you can check whether data are streamed for the end-effectors to be used as sensors (e.g. `/wholeBodyDynamics/right_arm/endEffectorWrench:o`). 23 | - In the [configuration file](https://github.com/robotology/wearables/blob/master/app/xml/ICubWearableDevice.xml), the following parameters are set properly: 24 | - `ft-sensors`: wrenche measurements that have to be exposed as wearable data. Those measurements should be provided by `whole-body-dynamics` estimator, or directly from sensors measurement. 25 | - `joint-sensors`: joints for which the state has to be exposed as wearable data. 26 | 27 | 28 | #### Calibration 29 | The estimated wrench data computed by `whole-body-dynamics` may be characterized by an offset due to ft sensors meaasurement. 30 | If using the real-robot, the following command can be sent to remove the offset (while the robot is lifted from the ground): 31 | ``` 32 | yarp rpc /wholeBodyDynamics/rpc 33 | calib all 300 34 | ``` 35 | If instead simulated robot is used, the command is the following (while the robot is on the ground): 36 | ``` 37 | yarp rpc /wholeBodyDynamics/rpc 38 | resetOffset all 300 39 | ``` 40 | 41 | #### Data 42 | If the device is running correctly, the stream of wearable data can be read with: 43 | ``` 44 | /ICub/WearableData/data:o 45 | ``` -------------------------------------------------------------------------------- /interfaces/IWear/include/Wearable/IWear/Sensors/IPoseSensor.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #ifndef WEARABLE_IPOSE_SENSOR_H 5 | #define WEARABLE_IPOSE_SENSOR_H 6 | 7 | #include "Wearable/IWear/Sensors/ISensor.h" 8 | #include "Wearable/IWear/Utils.h" 9 | 10 | namespace wearable { 11 | namespace sensor { 12 | class IPoseSensor; 13 | } // namespace sensor 14 | } // namespace wearable 15 | 16 | class wearable::sensor::IPoseSensor : public wearable::sensor::ISensor 17 | { 18 | public: 19 | IPoseSensor(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) 20 | : ISensor(aName, aStatus) 21 | { 22 | m_type = SensorType::PoseSensor; 23 | } 24 | 25 | virtual ~IPoseSensor() = default; 26 | 27 | virtual bool getPose(Quaternion& orientation, Vector3& position) const = 0; 28 | 29 | inline bool getPose(Vector7& pose) const; 30 | inline bool getPoseOrientationAsQuaternion(Quaternion& orientation) const; 31 | inline bool getPosePosition(Vector3& position) const; 32 | inline bool getPoseOrientationAsRotationMatrix(Matrix3& orientation) const; 33 | inline bool getPoseOrientationAsRPY(Vector3& orientation) const; 34 | 35 | inline static const std::string getPrefix(); 36 | }; 37 | 38 | inline const std::string wearable::sensor::IPoseSensor::getPrefix() 39 | { 40 | return "pose" + wearable::Separator; 41 | } 42 | 43 | inline bool wearable::sensor::IPoseSensor::getPose(Vector7& pose) const 44 | { 45 | Quaternion orientation; 46 | Vector3 position; 47 | if (!getPose(orientation, position)) { 48 | return false; 49 | } 50 | 51 | pose[0] = orientation[0]; 52 | pose[1] = orientation[1]; 53 | pose[2] = orientation[2]; 54 | pose[3] = orientation[3]; 55 | pose[4] = position[0]; 56 | pose[5] = position[1]; 57 | pose[6] = position[2]; 58 | 59 | return true; 60 | } 61 | 62 | bool wearable::sensor::IPoseSensor::getPoseOrientationAsQuaternion(Quaternion& orientation) const 63 | { 64 | Vector3 dummy; 65 | return getPose(orientation, dummy); 66 | } 67 | 68 | inline bool wearable::sensor::IPoseSensor::getPosePosition(Vector3& position) const 69 | { 70 | Quaternion dummy; 71 | return getPose(dummy, position); 72 | } 73 | 74 | inline bool 75 | wearable::sensor::IPoseSensor::getPoseOrientationAsRotationMatrix(Matrix3& orientation) const 76 | { 77 | Quaternion quat; 78 | if (!getPoseOrientationAsQuaternion(quat)) { 79 | return false; 80 | } 81 | orientation = utils::quaternionToRotationMatrix(quat); 82 | return true; 83 | }; 84 | 85 | inline bool wearable::sensor::IPoseSensor::getPoseOrientationAsRPY(Vector3& orientation) const 86 | { 87 | Quaternion quat; 88 | if (!getPoseOrientationAsQuaternion(quat)) { 89 | return false; 90 | } 91 | orientation = utils::quaternionToRPY(quat); 92 | return true; 93 | }; 94 | 95 | #endif // WEARABLE_IPOSE_SENSOR_H 96 | -------------------------------------------------------------------------------- /conf/xml/applications/HDERetargeting_iCub2_5_ifeel.xml: -------------------------------------------------------------------------------- 1 | 2 | HDERetargeting_iCub2_5_iFeel 3 | An application for running iCub2_5 retargeting with openxr and iFeel 4 | 5 | 6 | 7 | yarprobotinterface 8 | --config transformServer_to_iwear.xml 9 | 10 | /transformServer/rpc 11 | /transformServer/rpc 12 | /virtualizer/playerOrientation:o 13 | 14 | YARP_FORWARD_LOG_ENABLE=1 15 | Publish transforms as iWear 16 | localhost 17 | transformServer_to_iwear 18 | 19 | 20 | 21 | 22 | yarprobotinterface 23 | --config RobotStateProvider_iCub2_5_openxr_ifeel.xml 24 | 26 | YARP_FORWARD_LOG_ENABLE=1 27 | Run HDE kinemaics for iCub2_5 28 | localhost 29 | OpenXR-HumanStateProvider 30 | 31 | 32 | 33 | 34 | HumanStateVisualizer 35 | --from HumanStateVisualizer_iCub2_5_openxr_ifeel.ini 36 | 37 | /iCub/RobotStateServer/state:o 38 | 39 | YARP_FORWARD_LOG_ENABLE=1 40 | Run the iDynTree Visualizer 41 | localhost 42 | OpenXR-HumanStateVisualizer 43 | 44 | 45 | 46 | 47 | yarprobotinterface 48 | --config RobotPositionController_iCub2_5.xml 49 | 50 | /iCub/RobotStateServer/state:o 51 | 52 | YARP_FORWARD_LOG_ENABLE=1 53 | Run the iCub position controller 54 | localhost 55 | PositionController 56 | 57 | 58 | 59 | 60 | yarprobotinterface 61 | --config HapticGlove.xml 62 | YARP_FORWARD_LOG_ENABLE=0 63 | Run SenseGlove Wearable Device 64 | localhost 65 | SenseGlove 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /cmake/FindXsensXME.cmake: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | 4 | 5 | #.rst: 6 | # FindXsensXME 7 | # ----------- 8 | # 9 | # Find the Xsens MVN Engine (XME) library API. 10 | # 11 | # IMPORTED Targets 12 | # ^^^^^^^^^^^^^^^^ 13 | # 14 | # This module defines the following :prop_tgt:`IMPORTED` targets if 15 | # XME drivers has been found:: 16 | # 17 | # Xsens::XME 18 | # 19 | # Result Variables 20 | # ^^^^^^^^^^^^^^^^ 21 | # 22 | # This module defines the following variables:: 23 | # 24 | # XsensXME_FOUND - System has XME 25 | # XsensXME_INCLUDE_DIRS - Include directories for XME 26 | # XsensXME_LIBRARIES - libraries to link against XME 27 | # 28 | # Readed enviromental variables 29 | # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 30 | # 31 | # This module reads hints about search locations from variables:: 32 | # 33 | # XsensXME_ROOT - Directory containing the SDK files. 34 | 35 | if(WIN32) 36 | 37 | if(NOT CMAKE_SIZEOF_VOID_P STREQUAL "8") 38 | #32bit 39 | set(XME_ARCH "32") 40 | set(XME_DIR_PATH "Win32") 41 | else() 42 | #64bit 43 | set(XME_ARCH "64") 44 | set(XME_DIR_PATH "x64") 45 | endif() 46 | 47 | 48 | find_path(Xsens_INCLUDE_DIR xme.h 49 | HINTS "$ENV{XsensXME_ROOT}/${XME_DIR_PATH}/include") 50 | 51 | find_library(Xsens_xme_LIBRARY "xme${XME_ARCH}" 52 | HINTS "$ENV{XsensXME_ROOT}/${XME_DIR_PATH}/lib") 53 | 54 | find_library(Xsens_xstypes_LIBRARY "xstypes${XME_ARCH}" 55 | HINTS "$ENV{XsensXME_ROOT}/${XME_DIR_PATH}/lib") 56 | 57 | 58 | mark_as_advanced(Xsens_INCLUDE_DIR 59 | Xsens_xme_LIBRARY 60 | Xsens_xstypes_LIBRARY) 61 | 62 | if (Xsens_xme_LIBRARY 63 | AND Xsens_xstypes_LIBRARY 64 | AND Xsens_INCLUDE_DIR 65 | AND NOT TARGET Xsens::XME) 66 | 67 | add_library(Xsens::XME UNKNOWN IMPORTED) 68 | 69 | set_target_properties(Xsens::XME PROPERTIES 70 | INTERFACE_INCLUDE_DIRECTORIES "${Xsens_INCLUDE_DIR}" 71 | IMPORTED_LINK_INTERFACE_LANGUAGES "C" 72 | IMPORTED_LOCATION "${Xsens_xme_LIBRARY}" 73 | INTERFACE_LINK_LIBRARIES "${Xsens_xstypes_LIBRARY}") 74 | 75 | set(XsensXME_LIBRARIES Xsens::XME) 76 | set(XsensXME_INCLUDE_DIRS "${Xsens_INCLUDE_DIR}") 77 | endif() 78 | else() 79 | set(XsensXME_FOUND FALSE) 80 | endif() 81 | 82 | 83 | include(FindPackageHandleStandardArgs) 84 | find_package_handle_standard_args(XsensXME 85 | FOUND_VAR XsensXME_FOUND 86 | REQUIRED_VARS XsensXME_LIBRARIES XsensXME_INCLUDE_DIRS) 87 | 88 | # Set package properties if FeatureSummary was included 89 | if(COMMAND set_package_properties) 90 | set_package_properties(XsensXME PROPERTIES DESCRIPTION "API for Xsens MVN suit" 91 | URL "https://www.xsens.com/products/xsens-mvn/") 92 | endif() 93 | --------------------------------------------------------------------------------