diff --git a/CMakeLists.txt b/CMakeLists.txt index 0dfcd3be..f45d686b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,6 +11,7 @@ project(walking-teleoperation list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) include(WalkingTeleoperationFindDependencies) +include(InstallIniFiles) add_subdirectory(modules) add_subdirectory(app) diff --git a/app/scripts/OpenXR-KynDynRetargeting.xml b/app/scripts/OpenXR-KynDynRetargeting.xml new file mode 100644 index 00000000..d95d6f3e --- /dev/null +++ b/app/scripts/OpenXR-KynDynRetargeting.xml @@ -0,0 +1,49 @@ + + + + + + + OpenXR-KynDynRetargeting + Application to run kinematic and dynamic related modules for retargeting. + 1.0 + + Giulio Romualdi + + + + + WalkingModule + icub-head + --from dcm_walking_hand_retargeting.ini + + + + OpenXRModule + icub-console-gui + + /transformServer/transforms:o + /joypadDevice/Oculus/rpc:i + /icub/cam/left + /icub/cam/right + + + + + + /oculusRetargeting/leftHandPose:o + /walking-coordinator/leftHandDesiredPose:i + + + + /oculusRetargeting/rightHandPose:o + /walking-coordinator/rightHandDesiredPose:i + + + + /oculusRetargeting/walkingRpc + /walking-coordinator/rpc + + + + diff --git a/app/scripts/dcmWalkingRetargetingOpenXR.xml b/app/scripts/dcmWalkingRetargetingOpenXR.xml new file mode 100644 index 00000000..69aa6352 --- /dev/null +++ b/app/scripts/dcmWalkingRetargetingOpenXR.xml @@ -0,0 +1,34 @@ + + + + + + + OpenXR application + Application to run the retargeting with openXR. + 1.0 + + Giulio Romualdi + + + + transformServer + + + + Xprize-AudioRetargeting + + + + Xprize-VisualRetargeting + + + + OpenXR-KynDynRetargeting + + + + Xprize-PsychophysiologicalRetargeting + + + diff --git a/cmake/InstallIniFiles.cmake b/cmake/InstallIniFiles.cmake new file mode 100644 index 00000000..4983b9d2 --- /dev/null +++ b/cmake/InstallIniFiles.cmake @@ -0,0 +1,71 @@ +# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +# List the subdirectory +# http://stackoverflow.com/questions/7787823/cmake-how-to-get-the-name-of-all-subdirectories-of-a-directory +macro(SUBDIRLIST result curdir) + file(GLOB children RELATIVE ${curdir} ${curdir}/*) + set(dirlist "") + foreach(child ${children}) + if(IS_DIRECTORY ${curdir}/${child}) + list(APPEND dirlist ${child}) + endif() + endforeach() + set(${result} ${dirlist}) +endmacro() + +option(WALKING_TELEOPERATION_INSTALL_ALL_ROBOTS_INI "Enable installation of THE ini files for all robots" ON) +set(ROBOT_NAME "$ENV{YARP_ROBOT_NAME}" CACHE STRING "Name of your robot") + +function (install_ini_files) + + set(option) + set(oneValueArgs NAME PARENT_DIR) + set(multiValueArgs) + + set(prefix "walking_teleoperation") + + cmake_parse_arguments(${prefix} + "${options}" + "${oneValueArgs}" + "${multiValueArgs}" + ${ARGN}) + + set(name ${${prefix}_NAME}) + set(parent_dir ${${prefix}_PARENT_DIR}) + + yarp_configure_external_installation(${name}) + + # required by yarp + string(TOUPPER ${name} name_upper) + + # Get list of models + if(WALKING_TELEOPERATION_INSTALL_ALL_ROBOTS_INI) + subdirlist(robots ${parent_dir}/robots/) + + # Install each model + foreach (robot ${robots}) + file(GLOB scripts ${parent_dir}/robots/${robot}/*.ini) + yarp_install(FILES ${scripts} DESTINATION ${${name_upper}_ROBOTS_INSTALL_DIR}/${robot}) + + subdirlist(subdirs ${parent_dir}/robots/${robot}/) + foreach (subdir ${subdirs}) + yarp_install(DIRECTORY ${parent_dir}/robots/${robot}/${subdir} DESTINATION ${${name_upper}_ROBOTS_INSTALL_DIR}/${robot}) + endforeach () + endforeach () + else() + if(ROBOT_NAME) + if(IS_DIRECTORY "${parent_dir}/robots/${ROBOT_NAME}") + + file(GLOB scripts ${parent_dir}/robots/${ROBOT_NAME}/*.ini) + yarp_install(FILES ${scripts} DESTINATION ${${name_upper}_ROBOTS_INSTALL_DIR}/${ROBOT_NAME}) + + subdirlist(subdirs ${parent_dir}/robots/${ROBOT_NAME}/) + foreach (subdir ${subdirs}) + yarp_install(DIRECTORY robots/${ROBOT_NAME}/${subdir} DESTINATION ${${name_upper}_ROBOTS_INSTALL_DIR}/${ROBOT_NAME}) + endforeach () + endif() + endif() + endif() +endfunction() diff --git a/modules/CMakeLists.txt b/modules/CMakeLists.txt index a7b9fcf9..86f4e0fe 100644 --- a/modules/CMakeLists.txt +++ b/modules/CMakeLists.txt @@ -3,7 +3,9 @@ # Authors: Giulio Romualdi add_subdirectory(Utils) +add_subdirectory(Common) add_subdirectory(Oculus_module) +add_subdirectory(OpenXR_module) if(WALKING_TELEOPERATION_COMPILE_XsensModule) add_subdirectory(Xsens_module) diff --git a/modules/Common/CMakeLists.txt b/modules/Common/CMakeLists.txt new file mode 100644 index 00000000..3c13435b --- /dev/null +++ b/modules/Common/CMakeLists.txt @@ -0,0 +1,46 @@ +# Copyright (C) 2021 Fondazione Istituto Italiano di Tecnologia (IIT) +# All Rights Reserved. +# Authors: Giulio Romualdi + +# set target name +set(LIBRARY_NAME CommonLibrary) + +option(ENABLE_RPATH "Enable RPATH for this library" ON) +mark_as_advanced(ENABLE_RPATH) +include(AddInstallRPATHSupport) +add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_BINDIR}" + LIB_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}" + INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}" + DEPENDS ENABLE_RPATH + USE_LINK_PATH) + +# set cpp files +set(${LIBRARY_NAME}_SRC + src/FingersRetargeting.cpp + src/HandRetargeting.cpp + src/HeadRetargeting.cpp + src/RobotControlHelper.cpp + src/RetargetingController.cpp + ) + +# set hpp files +set(${LIBRARY_NAME}_HDR + include/FingersRetargeting.hpp + include/HandRetargeting.hpp + include/HeadRetargeting.hpp + include/RobotControlHelper.hpp + include/RetargetingController.hpp + ) + +# add an executable to the project using the specified source files. +add_library(${LIBRARY_NAME} ${${LIBRARY_NAME}_SRC} ${${LIBRARY_NAME}_HDR}) + +# add include directories to the build. +target_include_directories(${LIBRARY_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) + +target_link_libraries(${LIBRARY_NAME} + ${YARP_LIBRARIES} + ${iDynTree_LIBRARIES} + ctrlLib + UtilityLibrary + Eigen3::Eigen) diff --git a/modules/Oculus_module/include/FingersRetargeting.hpp b/modules/Common/include/FingersRetargeting.hpp similarity index 100% rename from modules/Oculus_module/include/FingersRetargeting.hpp rename to modules/Common/include/FingersRetargeting.hpp diff --git a/modules/Oculus_module/include/HandRetargeting.hpp b/modules/Common/include/HandRetargeting.hpp similarity index 100% rename from modules/Oculus_module/include/HandRetargeting.hpp rename to modules/Common/include/HandRetargeting.hpp diff --git a/modules/Oculus_module/include/HeadRetargeting.hpp b/modules/Common/include/HeadRetargeting.hpp similarity index 100% rename from modules/Oculus_module/include/HeadRetargeting.hpp rename to modules/Common/include/HeadRetargeting.hpp diff --git a/modules/Oculus_module/include/RetargetingController.hpp b/modules/Common/include/RetargetingController.hpp similarity index 100% rename from modules/Oculus_module/include/RetargetingController.hpp rename to modules/Common/include/RetargetingController.hpp diff --git a/modules/Oculus_module/include/RobotControlHelper.hpp b/modules/Common/include/RobotControlHelper.hpp similarity index 100% rename from modules/Oculus_module/include/RobotControlHelper.hpp rename to modules/Common/include/RobotControlHelper.hpp diff --git a/modules/Oculus_module/src/FingersRetargeting.cpp b/modules/Common/src/FingersRetargeting.cpp similarity index 100% rename from modules/Oculus_module/src/FingersRetargeting.cpp rename to modules/Common/src/FingersRetargeting.cpp diff --git a/modules/Oculus_module/src/HandRetargeting.cpp b/modules/Common/src/HandRetargeting.cpp similarity index 100% rename from modules/Oculus_module/src/HandRetargeting.cpp rename to modules/Common/src/HandRetargeting.cpp diff --git a/modules/Oculus_module/src/HeadRetargeting.cpp b/modules/Common/src/HeadRetargeting.cpp similarity index 100% rename from modules/Oculus_module/src/HeadRetargeting.cpp rename to modules/Common/src/HeadRetargeting.cpp diff --git a/modules/Oculus_module/src/RetargetingController.cpp b/modules/Common/src/RetargetingController.cpp similarity index 100% rename from modules/Oculus_module/src/RetargetingController.cpp rename to modules/Common/src/RetargetingController.cpp diff --git a/modules/Oculus_module/src/RobotControlHelper.cpp b/modules/Common/src/RobotControlHelper.cpp similarity index 100% rename from modules/Oculus_module/src/RobotControlHelper.cpp rename to modules/Common/src/RobotControlHelper.cpp diff --git a/modules/Oculus_module/CMakeLists.txt b/modules/Oculus_module/CMakeLists.txt index 669b899c..d500d373 100644 --- a/modules/Oculus_module/CMakeLists.txt +++ b/modules/Oculus_module/CMakeLists.txt @@ -1,4 +1,4 @@ -# Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia (IIT) +# Copyright (C) 2021 Fondazione Istituto Italiano di Tecnologia (IIT) # All Rights Reserved. # Authors: Giulio Romualdi @@ -27,21 +27,11 @@ include(FindPackageHandleStandardArgs) # set cpp files set(${EXE_TARGET_NAME}_SRC src/main.cpp - src/FingersRetargeting.cpp - src/HandRetargeting.cpp - src/HeadRetargeting.cpp - src/RobotControlHelper.cpp - src/RetargetingController.cpp src/OculusModule.cpp ) # set hpp files set(${EXE_TARGET_NAME}_HDR - include/FingersRetargeting.hpp - include/HandRetargeting.hpp - include/HeadRetargeting.hpp - include/RobotControlHelper.hpp - include/RetargetingController.hpp include/OculusModule.hpp ) @@ -54,18 +44,19 @@ include_directories( add_executable(${EXE_TARGET_NAME} ${${EXE_TARGET_NAME}_SRC} ${${EXE_TARGET_NAME}_HDR}) if(ENABLE_LOGGER) - target_link_libraries(${EXE_TARGET_NAME} LINK_PUBLIC + target_link_libraries(${EXE_TARGET_NAME} PRIVATE ${YARP_LIBRARIES} ${iDynTree_LIBRARIES} ctrlLib - UtilityLibrary + CommonLibrary matlogger2::matlogger2) else(ENABLE_LOGGER) - target_link_libraries(${EXE_TARGET_NAME} LINK_PUBLIC + target_link_libraries(${EXE_TARGET_NAME} PRIVATE ${YARP_LIBRARIES} ${iDynTree_LIBRARIES} ctrlLib UtilityLibrary + CommonLibrary ) endif() diff --git a/modules/OpenXR_module/CMakeLists.txt b/modules/OpenXR_module/CMakeLists.txt new file mode 100644 index 00000000..67caa960 --- /dev/null +++ b/modules/OpenXR_module/CMakeLists.txt @@ -0,0 +1,66 @@ +# Copyright (C) 2021 Fondazione Istituto Italiano di Tecnologia (IIT) +# All Rights Reserved. +# Authors: Giulio Romualdi + +# set target name +set(EXE_TARGET_NAME OpenXRRetargetingModule) + +option(ENABLE_RPATH "Enable RPATH for this library" ON) +option(ENABLE_LOGGER "Enable logger using matlogger2" OFF) + +mark_as_advanced(ENABLE_RPATH) +include(AddInstallRPATHSupport) +add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_BINDIR}" + LIB_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}" + INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}" + DEPENDS ENABLE_RPATH + USE_LINK_PATH) + +# Find required package +if(ENABLE_LOGGER) + add_definitions(-DENABLE_LOGGER) + find_package(matlogger2 REQUIRED) +endif(ENABLE_LOGGER) +include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR}) +include(FindPackageHandleStandardArgs) + +# set cpp files +set(${EXE_TARGET_NAME}_SRC + src/main.cpp + src/OpenXRModule.cpp + ) + +# set hpp files +set(${EXE_TARGET_NAME}_HDR + include/OpenXRModule.hpp + ) + +# add include directories to the build. +include_directories( + ${CMAKE_CURRENT_SOURCE_DIR}/include +) + +# add an executable to the project using the specified source files. +add_executable(${EXE_TARGET_NAME} ${${EXE_TARGET_NAME}_SRC} ${${EXE_TARGET_NAME}_HDR}) + +if(ENABLE_LOGGER) + target_link_libraries(${EXE_TARGET_NAME} PRIVATE + ${YARP_LIBRARIES} + ${iDynTree_LIBRARIES} + ctrlLib + CommonLibrary + matlogger2::matlogger2) +else(ENABLE_LOGGER) + target_link_libraries(${EXE_TARGET_NAME} PRIVATE + ${YARP_LIBRARIES} + ${iDynTree_LIBRARIES} + ctrlLib + UtilityLibrary + CommonLibrary +) +endif() + +install(TARGETS ${EXE_TARGET_NAME} DESTINATION bin) + +install_ini_files(NAME WalkingTeleoperationOpenXR + PARENT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/config) diff --git a/modules/OpenXR_module/config/robots/iCubGazeboV3/headRetargetingParams.ini b/modules/OpenXR_module/config/robots/iCubGazeboV3/headRetargetingParams.ini new file mode 100644 index 00000000..a08d72eb --- /dev/null +++ b/modules/OpenXR_module/config/robots/iCubGazeboV3/headRetargetingParams.ini @@ -0,0 +1,8 @@ +remote_control_boards ("head") +# Notice the order of the joint list is not wrong. +# Indeed they are written according to the joint order of the icub-neck +joints_list ("neck_pitch", "neck_roll", "neck_yaw") + +smoothingTime 1.0 +PreparationSmoothingTime 3.0 +PreparationJointReferenceValues (0.0 , 0.0 , 0.0) diff --git a/modules/OpenXR_module/config/robots/iCubGazeboV3/leftFingersRetargetingParams.ini b/modules/OpenXR_module/config/robots/iCubGazeboV3/leftFingersRetargetingParams.ini new file mode 100644 index 00000000..901de4c1 --- /dev/null +++ b/modules/OpenXR_module/config/robots/iCubGazeboV3/leftFingersRetargetingParams.ini @@ -0,0 +1,6 @@ +remote_control_boards () +joints_list () + +useVelocity 1 + +fingersScaling () diff --git a/modules/OpenXR_module/config/robots/iCubGazeboV3/leftHandRetargetingParams.ini b/modules/OpenXR_module/config/robots/iCubGazeboV3/leftHandRetargetingParams.ini new file mode 100644 index 00000000..da696d84 --- /dev/null +++ b/modules/OpenXR_module/config/robots/iCubGazeboV3/leftHandRetargetingParams.ini @@ -0,0 +1,17 @@ +# additional rotations +# the following rotation map the hand oculus frame and the hand robot frame +handOculusFrame_R_handRobotFrame ((0.0 -1.0 0.0), (1.0 0.0 0.0), (0.0 0.0 1.0)) + +# the following rotation map the teleoperation frame and the teleoperation robot +# frame the Teleoperation robot frame chosen is "imu_frame" and according to iCub +# CAD has: +# The z-axis is parallel to gravity but pointing upwards. +# The x-axis points behind the robot. +# The y-axis points laterally and is chosen according to the right-hand rule. +# the Teleoperation frame is attached to the virtualizer and the origin is the +# same oculus inertial frame. In other words: +# The z-axis is parallel to gravity but pointing upwards +# The x-axis points forward +# The y-axis points laterally and is chosen according to the right-hand rule. +# So the rotation matrix is simply given by Rotz(pi) +teleoperationRobotFrame_R_teleoperationFrame ((0.0 0.0 -1.0), (-1.0 0.0 0.0), (0.0 1.0 0.0)) diff --git a/modules/OpenXR_module/config/robots/iCubGazeboV3/openXRConfig.ini b/modules/OpenXR_module/config/robots/iCubGazeboV3/openXRConfig.ini new file mode 100644 index 00000000..7588b235 --- /dev/null +++ b/modules/OpenXR_module/config/robots/iCubGazeboV3/openXRConfig.ini @@ -0,0 +1,71 @@ +name oculusRetargeting + +# ports +leftHandPosePort /leftHandPose:o +rightHandPosePort /rightHandPose:o +rpcWalkingPort_name /walkingRpc + +[GENERAL] +samplingTime 0.01 +robot icubSim +enableMoveRobot 1 +# the following value is a threshold used to update the teleoperation frame position +# when the human rotates inside the virtualizer +playerOrientationThreshold 0.2 + +# For kinematic scaling for task-space retargeting +humanHeight 1.76 +# The robot arm span is the distance between the left and right index fingertips when the arms are aligned, i.e. T pose. +robotArmSpan 1.0 +# If resetCameras !=0, the module will attempt to reset the cameras during the initialization phase. +resetCameras 0 +# The leftCameraPort and rightCameraPort are needed only if resetCameras is 1 +leftCameraPort /icub/cam/left +rightCameraPort /icub/cam/right + + +# include head parameters +[include HEAD_RETARGETING "headRetargetingParams.ini"] + +# include fingers parameters +[include LEFT_FINGERS_RETARGETING "leftFingersRetargetingParams.ini"] +[include RIGHT_FINGERS_RETARGETING "rightFingersRetargetingParams.ini"] + +# include hand parameters +[include LEFT_HAND_RETARGETING "leftHandRetargetingParams.ini"] +[include RIGHT_HAND_RETARGETING "rightHandRetargetingParams.ini"] + +[OPENXR] +root_frame_name openxr_origin +left_hand_frame_name openxr_left_hand +right_hand_frame_name openxr_right_hand +head_frame_name openxr_head +oculusOrientationPort /oculusOrientation:i +oculusPositionPort /oculusPosition:i + +move_icub_using_joypad 1 +deadzone 0.3 +fullscale 1.0 +scale_X 5.0 +scale_Y 5.0 + + +# {"/user/hand/left/input/menu/click", "vive_left_menu"}, +# {"/user/hand/left/input/trigger/click", "vive_left_trigger_click"}, +# {"/user/hand/left/input/squeeze/click", "vive_left_squeeze_click"}, +# {"/user/hand/left/input/trackpad/click", "vive_left_trackpad_click"}, +# {"/user/hand/right/input/menu/click", "vive_right_menu"}, +# {"/user/hand/right/input/trigger/click", "vive_right_trigger_click"}, +# {"/user/hand/right/input/squeeze/click", "vive_right_squeeze_click"}, +# {"/user/hand/right/input/trackpad/click", "vive_right_trackpad_click"} + +prepare_walking_buttons_map (-1, -1, -1, -1, 1, -1, -1, -1) # right menu ONLY +start_walking_buttons_map ( 1, -1, -1, -1, -1, -1, -1, -1) # left menu ONLY + +left_fingers_squeeze_buttons_map (-1, -1, 0, -1, -1, -1, -1, -1) # left squeeze deactivated +left_fingers_release_buttons_map (-1, -1, 1, -1, -1, -1, -1, -1) # left squeeze deactivated +left_walking_buttons_map (-1, -1, -1, 1, -1, -1, -1, -1) # left_trackpad_click activated + +right_fingers_squeeze_buttons_map (-1, -1, -1, -1, -1, -1, 0, -1) # right squeeze deactivated +right_fingers_release_buttons_map (-1, -1, -1, -1, -1, -1, 1, -1) # right squeeze deactivated +right_walking_buttons_map (-1, -1, -1, -1, -1, -1, -1, 1) # right_trackpad_click activated diff --git a/modules/OpenXR_module/config/robots/iCubGazeboV3/rightFingersRetargetingParams.ini b/modules/OpenXR_module/config/robots/iCubGazeboV3/rightFingersRetargetingParams.ini new file mode 100644 index 00000000..901de4c1 --- /dev/null +++ b/modules/OpenXR_module/config/robots/iCubGazeboV3/rightFingersRetargetingParams.ini @@ -0,0 +1,6 @@ +remote_control_boards () +joints_list () + +useVelocity 1 + +fingersScaling () diff --git a/modules/OpenXR_module/config/robots/iCubGazeboV3/rightHandRetargetingParams.ini b/modules/OpenXR_module/config/robots/iCubGazeboV3/rightHandRetargetingParams.ini new file mode 100644 index 00000000..da696d84 --- /dev/null +++ b/modules/OpenXR_module/config/robots/iCubGazeboV3/rightHandRetargetingParams.ini @@ -0,0 +1,17 @@ +# additional rotations +# the following rotation map the hand oculus frame and the hand robot frame +handOculusFrame_R_handRobotFrame ((0.0 -1.0 0.0), (1.0 0.0 0.0), (0.0 0.0 1.0)) + +# the following rotation map the teleoperation frame and the teleoperation robot +# frame the Teleoperation robot frame chosen is "imu_frame" and according to iCub +# CAD has: +# The z-axis is parallel to gravity but pointing upwards. +# The x-axis points behind the robot. +# The y-axis points laterally and is chosen according to the right-hand rule. +# the Teleoperation frame is attached to the virtualizer and the origin is the +# same oculus inertial frame. In other words: +# The z-axis is parallel to gravity but pointing upwards +# The x-axis points forward +# The y-axis points laterally and is chosen according to the right-hand rule. +# So the rotation matrix is simply given by Rotz(pi) +teleoperationRobotFrame_R_teleoperationFrame ((0.0 0.0 -1.0), (-1.0 0.0 0.0), (0.0 1.0 0.0)) diff --git a/modules/OpenXR_module/config/robots/iCubGenova09/headRetargetingParams.ini b/modules/OpenXR_module/config/robots/iCubGenova09/headRetargetingParams.ini new file mode 100644 index 00000000..a08d72eb --- /dev/null +++ b/modules/OpenXR_module/config/robots/iCubGenova09/headRetargetingParams.ini @@ -0,0 +1,8 @@ +remote_control_boards ("head") +# Notice the order of the joint list is not wrong. +# Indeed they are written according to the joint order of the icub-neck +joints_list ("neck_pitch", "neck_roll", "neck_yaw") + +smoothingTime 1.0 +PreparationSmoothingTime 3.0 +PreparationJointReferenceValues (0.0 , 0.0 , 0.0) diff --git a/modules/OpenXR_module/config/robots/iCubGenova09/leftFingersRetargetingParams.ini b/modules/OpenXR_module/config/robots/iCubGenova09/leftFingersRetargetingParams.ini new file mode 100644 index 00000000..9307768d --- /dev/null +++ b/modules/OpenXR_module/config/robots/iCubGenova09/leftFingersRetargetingParams.ini @@ -0,0 +1,6 @@ +remote_control_boards ("left_arm") +joints_list ("l_thumb_proximal", "l_thumb_distal", "l_index_proximal", "l_index-distal", "l_middle-proximal", "l_middle-distal", "l_little-fingers") + +useVelocity 1 + +fingersScaling (1, 3.5, 2, 3.5, 1, 3.5, 5) diff --git a/modules/OpenXR_module/config/robots/iCubGenova09/leftHandRetargetingParams.ini b/modules/OpenXR_module/config/robots/iCubGenova09/leftHandRetargetingParams.ini new file mode 100644 index 00000000..da696d84 --- /dev/null +++ b/modules/OpenXR_module/config/robots/iCubGenova09/leftHandRetargetingParams.ini @@ -0,0 +1,17 @@ +# additional rotations +# the following rotation map the hand oculus frame and the hand robot frame +handOculusFrame_R_handRobotFrame ((0.0 -1.0 0.0), (1.0 0.0 0.0), (0.0 0.0 1.0)) + +# the following rotation map the teleoperation frame and the teleoperation robot +# frame the Teleoperation robot frame chosen is "imu_frame" and according to iCub +# CAD has: +# The z-axis is parallel to gravity but pointing upwards. +# The x-axis points behind the robot. +# The y-axis points laterally and is chosen according to the right-hand rule. +# the Teleoperation frame is attached to the virtualizer and the origin is the +# same oculus inertial frame. In other words: +# The z-axis is parallel to gravity but pointing upwards +# The x-axis points forward +# The y-axis points laterally and is chosen according to the right-hand rule. +# So the rotation matrix is simply given by Rotz(pi) +teleoperationRobotFrame_R_teleoperationFrame ((0.0 0.0 -1.0), (-1.0 0.0 0.0), (0.0 1.0 0.0)) diff --git a/modules/OpenXR_module/config/robots/iCubGenova09/openXRConfig.ini b/modules/OpenXR_module/config/robots/iCubGenova09/openXRConfig.ini new file mode 100644 index 00000000..98d0081d --- /dev/null +++ b/modules/OpenXR_module/config/robots/iCubGenova09/openXRConfig.ini @@ -0,0 +1,72 @@ +name oculusRetargeting + +# ports +leftHandPosePort /leftHandPose:o +rightHandPosePort /rightHandPose:o +rpcWalkingPort_name /walkingRpc + +[GENERAL] +samplingTime 0.01 +robot icub +enableMoveRobot 1 +# the following value is a threshold used to update the teleoperation frame position +# when the human rotates inside the virtualizer +playerOrientationThreshold 0.2 + +# For kinematic scaling for task-space retargeting +humanHeight 1.76 +# The robot arm span is the distance between the left and right index fingertips when the arms are aligned, i.e. T pose. +robotArmSpan 1.0 +# If resetCameras !=0, the module will attempt to reset the cameras during the initialization phase. +resetCameras 0 +# The leftCameraPort and rightCameraPort are needed only if resetCameras is 1 +leftCameraPort /icub/cam/left +rightCameraPort /icub/cam/right + + +# include head parameters +[include HEAD_RETARGETING "headRetargetingParams.ini"] + +# include fingers parameters +[include LEFT_FINGERS_RETARGETING "leftFingersRetargetingParams.ini"] +[include RIGHT_FINGERS_RETARGETING "rightFingersRetargetingParams.ini"] + +# include hand parameters +[include LEFT_HAND_RETARGETING "leftHandRetargetingParams.ini"] +[include RIGHT_HAND_RETARGETING "rightHandRetargetingParams.ini"] + +[OPENXR] +root_frame_name openxr_origin +left_hand_frame_name openxr_left_hand +right_hand_frame_name openxr_right_hand +head_frame_name openxr_head +oculusOrientationPort /oculusOrientation:i +oculusPositionPort /oculusPosition:i + +move_icub_using_joypad 1 +deadzone 0.3 +fullscale 1.0 +scale_X 5.0 +scale_Y 5.0 + + +# {"/user/hand/left/input/menu/click", "vive_left_menu"}, +# {"/user/hand/left/input/trigger/click", "vive_left_trigger_click"}, +# {"/user/hand/left/input/squeeze/click", "vive_left_squeeze_click"}, +# {"/user/hand/left/input/trackpad/click", "vive_left_trackpad_click"}, +# {"/user/hand/right/input/menu/click", "vive_right_menu"}, +# {"/user/hand/right/input/trigger/click", "vive_right_trigger_click"}, +# {"/user/hand/right/input/squeeze/click", "vive_right_squeeze_click"}, +# {"/user/hand/right/input/trackpad/click", "vive_right_trackpad_click"} + + +prepare_walking_buttons_map (-1, -1, -1, -1, 1, -1, -1, -1) # right menu ONLY +start_walking_buttons_map ( 1, -1, -1, -1, -1, -1, -1, -1) # left menu ONLY + +left_fingers_squeeze_buttons_map (-1, -1, 0, -1, -1, -1, -1, -1) # left squeeze deactivated +left_fingers_release_buttons_map (-1, -1, 1, -1, -1, -1, -1, -1) # left squeeze deactivated +left_walking_buttons_map (-1, -1, -1, 1, -1, -1, -1, -1) # left_trackpad_click activated + +right_fingers_squeeze_buttons_map (-1, -1, -1, -1, -1, -1, 0, -1) # right squeeze deactivated +right_fingers_release_buttons_map (-1, -1, -1, -1, -1, -1, 1, -1) # right squeeze deactivated +right_walking_buttons_map (-1, -1, -1, -1, -1, -1, -1, 1) # right_trackpad_click activated diff --git a/modules/OpenXR_module/config/robots/iCubGenova09/rightFingersRetargetingParams.ini b/modules/OpenXR_module/config/robots/iCubGenova09/rightFingersRetargetingParams.ini new file mode 100644 index 00000000..d4af0664 --- /dev/null +++ b/modules/OpenXR_module/config/robots/iCubGenova09/rightFingersRetargetingParams.ini @@ -0,0 +1,6 @@ +remote_control_boards ("right_arm") +joints_list ("r_thumb_proximal", "r_thumb_distal", "r_index_proximal", "r_index-distal", "r_middle-proximal", "r_middle-distal", "r_little-fingers") + +useVelocity 1 + +fingersScaling (1, 3.5, 2, 3.5, 1, 3.5, 5) diff --git a/modules/OpenXR_module/config/robots/iCubGenova09/rightHandRetargetingParams.ini b/modules/OpenXR_module/config/robots/iCubGenova09/rightHandRetargetingParams.ini new file mode 100644 index 00000000..da696d84 --- /dev/null +++ b/modules/OpenXR_module/config/robots/iCubGenova09/rightHandRetargetingParams.ini @@ -0,0 +1,17 @@ +# additional rotations +# the following rotation map the hand oculus frame and the hand robot frame +handOculusFrame_R_handRobotFrame ((0.0 -1.0 0.0), (1.0 0.0 0.0), (0.0 0.0 1.0)) + +# the following rotation map the teleoperation frame and the teleoperation robot +# frame the Teleoperation robot frame chosen is "imu_frame" and according to iCub +# CAD has: +# The z-axis is parallel to gravity but pointing upwards. +# The x-axis points behind the robot. +# The y-axis points laterally and is chosen according to the right-hand rule. +# the Teleoperation frame is attached to the virtualizer and the origin is the +# same oculus inertial frame. In other words: +# The z-axis is parallel to gravity but pointing upwards +# The x-axis points forward +# The y-axis points laterally and is chosen according to the right-hand rule. +# So the rotation matrix is simply given by Rotz(pi) +teleoperationRobotFrame_R_teleoperationFrame ((0.0 0.0 -1.0), (-1.0 0.0 0.0), (0.0 1.0 0.0)) diff --git a/modules/OpenXR_module/include/OpenXRModule.hpp b/modules/OpenXR_module/include/OpenXRModule.hpp new file mode 100644 index 00000000..4fa07b9e --- /dev/null +++ b/modules/OpenXR_module/include/OpenXRModule.hpp @@ -0,0 +1,65 @@ +/** + * @file OpenXRModule.hpp + * @authors Giulio Romualdi + * @copyright 2021 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2021 + */ + +#ifndef OPENXR_MODULE_HPP +#define OPENXR_MODULE_HPP + +// std +#include +#include + +// YARP +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +class OpenXRModule : public yarp::os::RFModule +{ +private: + struct Impl; + std::unique_ptr m_pImpl; + +public: + OpenXRModule(); + ~OpenXRModule(); + /** + * Get the period of the RFModule. + * @return the period of the module. + */ + double getPeriod() final; + + /** + * Main function of the RFModule. + * @return true in case of success and false otherwise. + */ + bool updateModule() final; + + /** + * Configure the RFModule. + * @param rf is the reference to a resource finder object + * @return true in case of success and false otherwise. + */ + bool configure(yarp::os::ResourceFinder& rf) final; + + /** + * Close the RFModule. + * @return true in case of success and false otherwise. + */ + bool close() final; +}; + +#endif // OPENXR diff --git a/modules/OpenXR_module/src/OpenXRModule.cpp b/modules/OpenXR_module/src/OpenXRModule.cpp new file mode 100644 index 00000000..4bf87bde --- /dev/null +++ b/modules/OpenXR_module/src/OpenXRModule.cpp @@ -0,0 +1,1265 @@ +/** + * @file OpenXRModule.cpp + * @authors Giulio Romualdi + * @copyright 2021 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2021 + */ + +#include +#include +#include +#include + +// YARP +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +Eigen::Ref> getRotation(yarp::sig::Matrix& m) +{ + return Eigen::Map>(m.data()).topLeftCorner<3, 3>(); +} + +auto getPosition(yarp::sig::Matrix& m) +{ + return Eigen::Map>(m.data()) + .topRightCorner<3, 1>(); +} + +auto getPosition(const yarp::sig::Matrix& m) +{ + return Eigen::Map>(m.data()) + .topRightCorner<3, 1>(); +} + +yarp::sig::Matrix identitySE3() +{ + yarp::sig::Matrix m; + m.resize(4, 4); + getRotation(m).setIdentity(); + getPosition(m).setZero(); + m(3, 3) = 1; + return m; +} + +struct OpenXRModule::Impl +{ + struct JoypadParameters + { + double deadzone; /**< Joypad deadzone */ + double fullscale; /**< Joypad fullscale */ + double scaleX; /**< Scaling factor on the x axis */ + double scaleY; /**< Scaling factor on the y axis */ + double x; /**< x value */ + double y; /**< y value */ + + std::vector startWalkingButtonsMap; + std::vector prepareWalkingButtonsMap; + std::vector stopWalkingButtonsMap; + std::vector leftFingersSqueezeButtonsMap; + std::vector leftFingersReleaseButtonsMap; + std::vector rightFingersSqueezeButtonsMap; + std::vector rightFingersReleaseButtonsMap; + + int xJoypadIndex; /**< Mapping of the axis related to x coordinate */ + int yJoypadIndex; /**< Mapping of the axis related to y coordinate */ + std::vector joypadButtonsMap; + + int fingersVelocityLeftIndex; /**< Index of the trigger used for squeezing the left hand */ + int fingersVelocityRightIndex; /**< Index of the trigger used for + squeezing the right hand */ + }; + JoypadParameters joypadParameters; + + enum class OpenXRFSM + { + Configured, + Running, + InPreparation + }; + OpenXRFSM state; /**< State of the OpenXRFSM */ + + std::unique_ptr neckJointsPreparationSmoother{nullptr}; + double preparationSmoothingTime; + double dT; + unsigned actuatedDOFs; + double playerOrientationThreshold; + + std::unique_ptr head; /**< Pointer to the head retargeting object. */ + std::unique_ptr leftHandFingers; /**< Pointer to the left + finger retargeting object. */ + std::unique_ptr rightHandFingers; /**< Pointer to the right + finger retargeting object. */ + std::unique_ptr rightHand; /**< Pointer to the right + hand retargeting object. */ + std::unique_ptr leftHand; /**< Pointer to the left hand + retargeting object. */ + + yarp::os::BufferedPort leftHandPosePort; /**< Left hand port pose. */ + yarp::os::BufferedPort rightHandPosePort; /**< Right hand port pose. */ + + yarp::os::RpcClient rpcWalkingClient; /**< Rpc client used for sending command to the walking + controller */ + + // transform server + yarp::dev::PolyDriver transformClientDevice; /**< Transform client. */ + yarp::dev::IFrameTransform* frameTransformInterface{nullptr}; /**< Frame transform + interface. */ + yarp::dev::PolyDriver joypadDevice; /**< Joypad polydriver. */ + yarp::dev::IJoypadController* joypadControllerInterface{nullptr}; /**< joypad interface. */ + + std::string rootFrameName; /**< Name of the root frame used in the transform server */ + std::string headFrameName; /**< Name of the head frame used in the transform server */ + std::string leftHandFrameName; /**< Name of the left hand frame used in the transform server */ + std::string rightHandFrameName; /**< Name of the right hand + frame used in the transform server */ + + yarp::sig::Matrix openXRRoot_T_lOpenXR; + yarp::sig::Matrix openXRRoot_T_rOpenXR; + yarp::sig::Matrix openXRRoot_T_headOpenXR; + yarp::sig::Matrix openXRInitialAlignement; + + std::vector openXRHeadsetPoseInertial; + + bool moveRobot{false}; + double playerOrientation{0}; /**< Player orientation (read by the Virtualizer) + only yaw. */ + + bool leftAndRightSwapped{false}; + std::vector buttonsState; + + bool isButtonStateEqualToMask(const std::vector& mask) const + { + return (mask.size() == this->buttonsState.size() + && std::equal(mask.begin(), + mask.end(), + this->buttonsState.begin(), + [](const auto& a, const auto& b) { + if (a < 0) + return true; + return a == b; + })); + } + + void initializeNeckJointsSmoother(const unsigned actuatedDOFs, + const double dT, + const double smoothingTime, + const yarp::sig::Vector jointsInitialValue) + { + this->neckJointsPreparationSmoother = std::make_unique( + this->actuatedDOFs, this->dT, smoothingTime); + this->neckJointsPreparationSmoother->init(jointsInitialValue); + } + + void getNeckJointsRefSmoothedValues(yarp::sig::Vector& smoothedJointValues) + { + yarp::sig::Vector jointValues = {0.0, 0.0, 0.0}; + this->neckJointsPreparationSmoother->computeNextValues(jointValues); + smoothedJointValues = this->neckJointsPreparationSmoother->getPos(); + } + + bool getFeedbacks() + { + if (!this->head->controlHelper()->getFeedback()) + { + yError() << "[OpenXRModule::getFeedbacks] Unable to get the joint encoders feedback: " + "HeadRetargeting"; + return false; + } + this->head->controlHelper()->updateTimeStamp(); + + return true; + } + + bool getTransforms() + { + // check if everything is ok + if (!this->frameTransformInterface->frameExists(this->rootFrameName)) + { + yError() << "[OpenXRModule::getTransforms] No " << this->rootFrameName << " frame."; + return false; + } + + if (!this->frameTransformInterface->frameExists(this->headFrameName)) + { + yError() << "[OpenXRModule::getTransforms] If OpenXr is used the head transform " + "must be provided by the transform server."; + return false; + } + + if (!this->frameTransformInterface->getTransform(this->headFrameName, // + this->rootFrameName, + this->openXRRoot_T_headOpenXR)) + { + yError() << "[OpenXRModule::getTransforms] Unable to evaluate the " + << this->headFrameName << " to " << this->rootFrameName << "transformation"; + return false; + } + + // This is to remove any initial misplacement and rotations around gravity + iDynTree::toEigen(this->openXRRoot_T_headOpenXR) + = iDynTree::toEigen(this->openXRInitialAlignement) + * iDynTree::toEigen(this->openXRRoot_T_headOpenXR); + + iDynTree::Rotation temp; + iDynTree::toEigen(temp) + = iDynTree::toEigen(this->openXRRoot_T_headOpenXR).topLeftCorner<3, 3>(); + + temp.getRPY(this->openXRHeadsetPoseInertial[3], + this->openXRHeadsetPoseInertial[4], + this->openXRHeadsetPoseInertial[5]); + // this->openXRHeadsetPoseInertial is a 6d std::vector here I'm getting the first three + // elements + Eigen::Map(this->openXRHeadsetPoseInertial.data()) + = getPosition(this->openXRRoot_T_headOpenXR); + + if (!this->frameTransformInterface->frameExists(this->leftHandFrameName)) + { + + yError() << "[OpenXRModule::getTransforms] No " << this->leftHandFrameName << "frame."; + return false; + } + + if (!this->frameTransformInterface->frameExists(this->rightHandFrameName)) + { + yError() << "[OpenXRModule::getTransforms] No " << this->rightHandFrameName + << " frame."; + return false; + } + + if (!this->frameTransformInterface->getTransform(this->leftHandFrameName, // + this->rootFrameName, + this->openXRRoot_T_lOpenXR)) + { + yError() << "[OpenXRModule::getTransforms] Unable to evaluate the " + << this->leftHandFrameName << " to " << this->rootFrameName + << "transformation"; + return false; + } + + if (!this->frameTransformInterface->getTransform(this->rightHandFrameName, // + this->rootFrameName, + this->openXRRoot_T_rOpenXR)) + { + yError() << "[OpenXRModule::getTransforms] Unable to evaluate the " + << this->rightHandFrameName << " to " << this->rootFrameName + << "transformation"; + return false; + } + + // This is to remove any initial misplacement and rotations around gravity + iDynTree::toEigen(this->openXRRoot_T_rOpenXR) + = iDynTree::toEigen(this->openXRInitialAlignement) + * iDynTree::toEigen(this->openXRRoot_T_rOpenXR); + + iDynTree::toEigen(this->openXRRoot_T_lOpenXR) + = iDynTree::toEigen(this->openXRInitialAlignement) + * iDynTree::toEigen(this->openXRRoot_T_lOpenXR); + + return true; + } + + bool configureTranformClient(const yarp::os::Searchable& config, + const std::string& applicationName) + { + yarp::os::Property options; + options.put("device", "transformClient"); + options.put("remote", "/transformServer"); + options.put("local", "/" + applicationName + "/transformClient"); + + if (!this->transformClientDevice.open(options)) + { + yError() << "[OpenXRModule::configureTranformClient] Unable " + "to open transformClient device"; + return false; + } + + // obtain the interface + if (!this->transformClientDevice.view(this->frameTransformInterface) + || this->frameTransformInterface == nullptr) + { + yError() << "[OpenXRModule::configureTranformClient] Cannot obtain Transform client."; + return false; + } + + if (!YarpHelper::getStringFromSearchable(config, "root_frame_name", this->rootFrameName)) + { + yError() << "[OpenXRModule::configureTranformClient] Cannot obtain Transform client."; + return false; + } + + if (!YarpHelper::getStringFromSearchable(config, "head_frame_name", this->headFrameName)) + { + + yError() << "[OpenXRModule::configureTranformClient] Seems that the head " + "orientation is not streamed through the transform server."; + return false; + } + + if (!YarpHelper::getStringFromSearchable(config, // + "left_hand_frame_name", + this->leftHandFrameName)) + { + yError() << "[OpenXRModule::configureTranformClient] Cannot obtain Transform client."; + return false; + } + + if (!YarpHelper::getStringFromSearchable(config, // + "right_hand_frame_name", + this->rightHandFrameName)) + { + yError() << "[OpenXRModule::configureTranformClient] Cannot obtain Transform client."; + return false; + } + + this->openXRRoot_T_lOpenXR = identitySE3(); + this->openXRRoot_T_rOpenXR = identitySE3(); + this->openXRRoot_T_headOpenXR = identitySE3(); + this->openXRInitialAlignement = identitySE3(); + + return true; + } + + bool configureJoypad(const yarp::os::Searchable& config, const std::string& name) + { + yarp::os::Property options; + options.put("device", "JoypadControlClient"); + options.put("remote", "/joypadDevice/Oculus"); + options.put("local", "/" + name + "/joypadControlClient"); + + if (!this->joypadDevice.open(options)) + { + yError() << "[OpenXRModule::configureJoypad] Unable to open the polydriver."; + return false; + } + + // get the interface + if (!this->joypadDevice.view(this->joypadControllerInterface) + || this->joypadControllerInterface == nullptr) + { + yError() << "[OpenXRModule::configureJoypad] Unable to attach JoypadController" + " interface to the PolyDriver object"; + return false; + } + + if (!YarpHelper::getDoubleFromSearchable(config, + "deadzone", // + this->joypadParameters.deadzone)) + { + yError() << "[OpenXRModule::configureJoypad] Unable to find parameter deadzone"; + return false; + } + + if (!YarpHelper::getDoubleFromSearchable(config, // + "fullscale", + this->joypadParameters.fullscale)) + { + yError() << "[OpenXRModule::configureJoypad] Unable to find parameter fullscale"; + return false; + } + + if (!YarpHelper::getDoubleFromSearchable(config, "scale_X", this->joypadParameters.scaleX)) + { + yError() << "[OpenXRModule::configureJoypad] Unable to find parameter scale_X"; + return false; + } + + if (!YarpHelper::getDoubleFromSearchable(config, "scale_Y", this->joypadParameters.scaleY)) + { + yError() << "[OpenXRModule::configureJoypad] Unable to find parameter scale_Y"; + return false; + } + + + // this vector of pointers of maps will simplify some checks later on + std::vector*> buttonsMap; + std::vector*> stateMachineButtonsMap; + + // set the index of the axis according to the OVRheadset yarp device + // The order of the buttons are here: https://github.com/ami-iit/yarp-device-openxrheadset/blob/b560d603bba8e50415be839d0e22b51219abbda8/src/devices/openxrheadset/OpenXrInterface.cpp#L651-L661 + + if (!YarpHelper::getIntVectorFromSearchable(config, // + "start_walking_buttons_map", + this->joypadParameters.startWalkingButtonsMap)) + { + yError() << "[OpenXRModule::configureJoypad] Unable to find parameter " + "start_walking_buttons_map"; + return false; + } + buttonsMap.push_back(&this->joypadParameters.startWalkingButtonsMap); + stateMachineButtonsMap.push_back(&this->joypadParameters.startWalkingButtonsMap); + + if (!YarpHelper::getIntVectorFromSearchable(config, // + "prepare_walking_buttons_map", + this->joypadParameters.prepareWalkingButtonsMap)) + { + yError() << "[OpenXRModule::configureJoypad] Unable to find parameter " + "prepare_walking_buttons_map"; + return false; + } + buttonsMap.push_back(&this->joypadParameters.prepareWalkingButtonsMap); + stateMachineButtonsMap.push_back(&this->joypadParameters.prepareWalkingButtonsMap); + + if (!YarpHelper::getIntVectorFromSearchable(config, // + "left_fingers_squeeze_buttons_map", + this->joypadParameters.leftFingersSqueezeButtonsMap)) + { + yError() << "[OpenXRModule::configureJoypad] Unable to find parameter " + "left_fingers_squeeze_buttons_map"; + return false; + } + buttonsMap.push_back(&this->joypadParameters.leftFingersSqueezeButtonsMap); + + if (!YarpHelper::getIntVectorFromSearchable(config, // + "left_fingers_release_buttons_map", + this->joypadParameters.leftFingersReleaseButtonsMap)) + { + yError() << "[OpenXRModule::configureJoypad] Unable to find parameter " + "left_fingers_release_buttons_map"; + return false; + } + buttonsMap.push_back(&this->joypadParameters.leftFingersReleaseButtonsMap); + + if (!YarpHelper::getIntVectorFromSearchable(config, // + "right_fingers_squeeze_buttons_map", + this->joypadParameters.rightFingersSqueezeButtonsMap)) + { + yError() << "[OpenXRModule::configureJoypad] Unable to find parameter " + "right_fingers_squeeze_buttons_map"; + return false; + } + buttonsMap.push_back(&this->joypadParameters.rightFingersSqueezeButtonsMap); + + if (!YarpHelper::getIntVectorFromSearchable(config, // + "right_fingers_release_buttons_map", + this->joypadParameters.rightFingersReleaseButtonsMap)) + { + yError() << "[OpenXRModule::configureJoypad] Unable to find parameter " + "right_fingers_release_buttons_map"; + return false; + } + buttonsMap.push_back(&this->joypadParameters.rightFingersReleaseButtonsMap); + + // check if the size of all the maps are the same. If not there is a mistake in the + // configuration file. + const auto size = this->joypadParameters.rightFingersReleaseButtonsMap.size(); + for (const auto map : buttonsMap) + { + if (size != map->size()) + { + yError() + << "[OpenXRModule::configureJoypad] Mismatch in the size of the buttons map. " + "Please check the configuration file"; + return false; + } + } + + std::vector leftWalkingButtonsMap; + if (!YarpHelper::getIntVectorFromSearchable(config, // + "left_walking_buttons_map", + leftWalkingButtonsMap)) + { + yError() << "[OpenXRModule::configureJoypad] Unable to find parameter " + "left_walking_buttons_map"; + return false; + } + buttonsMap.push_back(&leftWalkingButtonsMap); + + std::vector rightWalkingButtonsMap; + if (!YarpHelper::getIntVectorFromSearchable(config, // + "right_walking_buttons_map", + rightWalkingButtonsMap)) + { + yError() << "[OpenXRModule::configureJoypad] Unable to find parameter " + "right_walking_buttons_map"; + return false; + } + buttonsMap.push_back(&rightWalkingButtonsMap); + + + auto isEqual = [](const auto& v1, const auto& v2) { + return (v1->size() == v2->size() && std::equal(v1->begin(), v1->end(), v2->begin())); + }; + + for (auto it = stateMachineButtonsMap.cbegin(); it != stateMachineButtonsMap.cend(); + std::advance(it, 1)) + { + for (auto otherIt = std::next(it, 1); otherIt != stateMachineButtonsMap.cend(); + std::advance(otherIt, 1)) + { + if (isEqual(*it, *otherIt)) + { + yError() << "[OpenXRModule::configureJoypad] State machine maps cannot must be " + "different"; + return false; + } + } + } + + // the stop button mask is the end of the prepare and start mask + this->joypadParameters.stopWalkingButtonsMap.resize( + this->joypadParameters.startWalkingButtonsMap.size()); + for (int i = 0; i < this->joypadParameters.startWalkingButtonsMap.size(); i++) + { + this->joypadParameters.stopWalkingButtonsMap[i] + = (this->joypadParameters.startWalkingButtonsMap[i] > 0 + || this->joypadParameters.prepareWalkingButtonsMap[i] > 0) + ? 1 + : 0; + } + + // in the vive (yarp) we have the following axis + // [vive_left_trigger, vive_right_trigger, vive_left_trackpad_x, vive_left_trackpad_y, + // vive_right_trackpad_x, vive_right_trackpad_y] + + this->joypadParameters.fingersVelocityLeftIndex = !this->leftAndRightSwapped ? 0 : 1; + this->joypadParameters.fingersVelocityRightIndex = !this->leftAndRightSwapped ? 1 : 0; + + constexpr bool useLeftStick = true; + constexpr int leftXIndex = 2; + constexpr int leftYIndex = 3; + constexpr int rightXIndex = 4; + constexpr int rightYIndex = 5; + + // truth table + // use left | left_and_right_swapped | output + // T | T | use right index (F) + // T | F | use left index (T) + // F | T | use left index (T) + // F | F | use right index (F) + // this is the xor operator + // in c++ you can use !A != !B + this->joypadParameters.xJoypadIndex + = (!useLeftStick != !this->leftAndRightSwapped) ? leftXIndex : rightXIndex; + this->joypadParameters.yJoypadIndex + = (!useLeftStick != !this->leftAndRightSwapped) ? leftYIndex : rightYIndex; + this->joypadParameters.joypadButtonsMap = (!useLeftStick != !this->leftAndRightSwapped) + ? leftWalkingButtonsMap + : rightWalkingButtonsMap; + + // if swapped we have to swap all the maps + if (this->leftAndRightSwapped) + { + std::swap(this->joypadParameters.rightFingersReleaseButtonsMap, + this->joypadParameters.leftFingersReleaseButtonsMap); + std::swap(this->joypadParameters.rightFingersSqueezeButtonsMap, + this->joypadParameters.leftFingersSqueezeButtonsMap); + std::swap(this->joypadParameters.startWalkingButtonsMap, + this->joypadParameters.prepareWalkingButtonsMap); + } + + return true; + } + + bool setLeftAndRightSwappedFlag() + { + + auto readTransforms = [this](yarp::sig::Matrix& headOpenXR_T_leftHandOpenXR, + yarp::sig::Matrix& headOpenXR_T_rightHandOpenXR) { + bool ok = true; + + ok = ok && this->frameTransformInterface->frameExists(this->rootFrameName); + ok = ok && this->frameTransformInterface->frameExists(this->headFrameName); + ok = ok && this->frameTransformInterface->frameExists(this->leftHandFrameName); + ok = ok && this->frameTransformInterface->frameExists(this->rightHandFrameName); + + + ok = ok && this->frameTransformInterface->getTransform(this->rightHandFrameName, // + this->headFrameName, + headOpenXR_T_rightHandOpenXR); + + ok = ok && this->frameTransformInterface->getTransform(this->leftHandFrameName, // + this->headFrameName, + headOpenXR_T_leftHandOpenXR); + + return ok; + }; + + yarp::sig::Matrix headOpenXR_T_expectedLeftHandOpenXR; + yarp::sig::Matrix headOpenXR_T_expectedRightHandOpenXR; + headOpenXR_T_expectedLeftHandOpenXR.resize(4, 4); + headOpenXR_T_expectedRightHandOpenXR.resize(4, 4); + + // try to read the transform + std::size_t counter = 0; + constexpr unsigned int maxAttempt = 20; + while (!readTransforms(headOpenXR_T_expectedLeftHandOpenXR, headOpenXR_T_expectedRightHandOpenXR)) + { + if (++counter == maxAttempt) + { + yError() << "[OpenXRModule::setLeftAndRightJoypad] Unable to read the " + "transform client."; + return false; + } + + // Sleep for some while + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + + // check if the left joypad is on the left and the right is on the right. If not we have to + // swap the content of the left and right frame name variables + + // first of all we check that one joypad is on the left and the other one is on the right, + // The openxr frame is oriented as follows X on the right Y upward and Z backward + // for this reason we take the X coordinate + // L R + // | | <----- representation of the user from the top + // |---H---| H = head L = left R = right + // + // -|-----> x + // | + // | + // v z + const double expectedLeftXCoordinate = getPosition(headOpenXR_T_expectedLeftHandOpenXR)(0); + const double expectedRightXCoordinate = getPosition(headOpenXR_T_expectedRightHandOpenXR)(0); + + if (expectedLeftXCoordinate * expectedRightXCoordinate > 0) + { + yError() << "[OpenXRModule::setLeftAndRightJoypad] One joypad should be on the left " + "and the other one on the right"; + return false; + } + + // if the expectedLeftHandOpenXR x coordinate is positive, the left and the right joypad has + // been swapped. It is not a big deal the following flag is here for handling this case + // solve your hadake + this->leftAndRightSwapped = expectedLeftXCoordinate > 0; + if (this->leftAndRightSwapped) + { + std::swap(this->leftHandFrameName, this->rightHandFrameName); + } + + return true; + + } + + bool configureOpenXR(const yarp::os::Searchable& config, const std::string& name) + { + if (!this->configureTranformClient(config, name)) + { + yError() << "[OpenXRModule::configureOpenXR] Unable to configure the transform client."; + return false; + } + + // Once the vive is stared the left and right joypad are chosen. Since the joypad are + // exactly the same. We should consider in the application which is the left and the right + // joypad + if (!this->setLeftAndRightSwappedFlag()) + { + yError() << "[OpenXRModule::configureOpenXR] Unable to set the flag related " + "to the swap of the left and right joypad."; + return false; + } + + if (!this->configureJoypad(config, name)) + { + yError() << "[OpenXRModule::configureOpenXR] Unable to configure the joypad client."; + return false; + } + + return true; + } + + bool resetCamera(const std::string& cameraPort, const std::string& localPort) + { + yarp::dev::PolyDriver grabberDriver; + + yarp::os::Property config; + config.put("device", "remote_grabber"); + config.put("remote", cameraPort); + config.put("local", localPort); + + bool opened = grabberDriver.open(config); + if (!opened) + { + yError() << "[OpenXRModule::configure] Cannot open remote_grabber device on port " + << cameraPort << "."; + return false; + } + + yarp::dev::IFrameGrabberControlsDC1394* grabberInterface; + + if (!grabberDriver.view(grabberInterface) || !grabberInterface) + { + yError() << "[OpenXRModule::configure] RemoteGrabber does not have " + "IFrameGrabberControlDC1394 interface, please update yarp."; + return false; + } + + if (!grabberInterface->setResetDC1394()) + { + yError() << "[OpenXRModule::configure] Failed to reset the camera on port " + << cameraPort << "."; + return false; + } + + grabberDriver.close(); + + return true; + }; + + double deadzone(const double& input) + { + if (input >= 0) + { + if (input > this->joypadParameters.deadzone) + return (input - this->joypadParameters.deadzone) + / (this->joypadParameters.fullscale - this->joypadParameters.deadzone); + else + return 0.0; + } else + { + if (input < -this->joypadParameters.deadzone) + return (input + this->joypadParameters.deadzone) + / (this->joypadParameters.fullscale - this->joypadParameters.deadzone); + else + return 0.0; + } + } + + double evaluateDesiredFingersVelocity(const std::vector& fingersSqueezeButtonsMask, + const std::vector& fingersReleaseButtonsMask, + int fingersVelocityIndex) + { + double fingersVelocity; + this->joypadControllerInterface->getAxis(fingersVelocityIndex, fingersVelocity); + + if (fingersVelocity == 0) + return 0; + + if (this->isButtonStateEqualToMask(fingersSqueezeButtonsMask)) + { + return fingersVelocity; + } + + if (this->isButtonStateEqualToMask(fingersReleaseButtonsMask)) + { + return -fingersVelocity; + } + return 0; + } + + std::vector getDeviceButtonsState() + { + std::vector buttons; + unsigned int buttonCount = 0; + this->joypadControllerInterface->getButtonCount(buttonCount); + + float value; + for (unsigned int i = 0; i < buttonCount; i++) + { + this->joypadControllerInterface->getButton(i, value); + + if (value > 0) + buttons.push_back(1); + else + buttons.push_back(0); + } + + return buttons; + } +}; + +OpenXRModule::OpenXRModule() + : m_pImpl{new Impl()} {}; + +OpenXRModule::~OpenXRModule(){}; + + + +bool OpenXRModule::configure(yarp::os::ResourceFinder& rf) +{ + // check if the configuration file is empty + if (rf.isNull()) + { + yError() << "[OpenXRModule::configure] Empty configuration " + "for the OpenXRModule application."; + return false; + } + + yarp::os::Bottle& generalOptions = rf.findGroup("GENERAL"); + // get the period + m_pImpl->dT = generalOptions.check("samplingTime", yarp::os::Value(0.1)).asDouble(); + yInfo() << "[OpenXRModule::configure] sampling time: " << m_pImpl->dT; + + // check if move the robot + m_pImpl->moveRobot = generalOptions.check("enableMoveRobot", yarp::os::Value(1)).asBool(); + yInfo() << "[OpenXRModule::configure] move the robot: " << m_pImpl->moveRobot; + + // check if move the robot + m_pImpl->playerOrientationThreshold = generalOptions + .check("playerOrientationThreshold", // + yarp::os::Value(0.2)) + .asDouble(); + yInfo() << "[OpenXRModule::configure] player orientation threshold: " + << m_pImpl->playerOrientationThreshold; + + // set the module name + std::string name; + if (!YarpHelper::getStringFromSearchable(rf, "name", name)) + { + yError() << "[OpenXRModule::configure] Unable to get a string from a searchable"; + return false; + } + setName(name.c_str()); + + // configure the openxr device + const std::string headsetGroup = "OPENXR"; + const yarp::os::Bottle& openXROptions = rf.findGroup(headsetGroup); + if (!m_pImpl->configureOpenXR(openXROptions, getName())) + { + yError() << "[OpenXRModule::configure] Unable to configure the oculus"; + return false; + } + + m_pImpl->head = std::make_unique(); + yarp::os::Bottle& headOptions = rf.findGroup("HEAD_RETARGETING"); + headOptions.append(generalOptions); + if (!m_pImpl->head->configure(headOptions, getName())) + { + yError() << "[OpenXRModule::configure] Unable to initialize the head retargeting."; + return false; + } + + // configure fingers retargeting + m_pImpl->leftHandFingers = std::make_unique(); + yarp::os::Bottle& leftFingersOptions = rf.findGroup("LEFT_FINGERS_RETARGETING"); + leftFingersOptions.append(generalOptions); + if (!m_pImpl->leftHandFingers->configure(leftFingersOptions, getName())) + { + yError() << "[OpenXRModule::configure] Unable to initialize the left fingers retargeting."; + return false; + } + + m_pImpl->rightHandFingers = std::make_unique(); + yarp::os::Bottle& rightFingersOptions = rf.findGroup("RIGHT_FINGERS_RETARGETING"); + rightFingersOptions.append(generalOptions); + if (!m_pImpl->rightHandFingers->configure(rightFingersOptions, getName())) + { + yError() << "[OpenXRModule::configure] Unable to initialize the right fingers retargeting."; + return false; + } + + // configure hands retargeting + m_pImpl->leftHand = std::make_unique(); + yarp::os::Bottle& leftHandOptions = rf.findGroup("LEFT_HAND_RETARGETING"); + leftHandOptions.append(generalOptions); + if (!m_pImpl->leftHand->configure(leftHandOptions)) + { + yError() << "[OpenXRModule::configure] Unable to initialize the left fingers retargeting."; + return false; + } + + m_pImpl->rightHand = std::make_unique(); + yarp::os::Bottle& rightHandOptions = rf.findGroup("RIGHT_HAND_RETARGETING"); + rightHandOptions.append(generalOptions); + if (!m_pImpl->rightHand->configure(rightHandOptions)) + { + yError() << "[OpenXRModule::configure] Unable to initialize the right fingers retargeting."; + return false; + } + + // open ports + std::string portName; + if (!YarpHelper::getStringFromSearchable(rf, "leftHandPosePort", portName)) + { + yError() << "[OpenXRModule::configure] Unable to get a string from a searchable"; + return false; + } + if (!m_pImpl->leftHandPosePort.open("/" + getName() + portName)) + { + yError() << "[OpenXRModule::configure] Unable to open the port " << portName; + return false; + } + + if (!YarpHelper::getStringFromSearchable(rf, "rightHandPosePort", portName)) + { + yError() << "[OpenXRModule::configure] Unable to get a string from a searchable"; + return false; + } + if (!m_pImpl->rightHandPosePort.open("/" + getName() + portName)) + { + yError() << "[OpenXRModule::configure] Unable to open the port " << portName; + return false; + } + + // if (!m_imagesOrientationPort.open("/" + getName() + "/imagesOrientation:o")) + // { + // yError() << "[OpenXRModule::configure] Unable to open the port " << portName; + // return false; + // } + // if (!m_robotOrientationPort.open("/" + getName() + "/robotOrientation:i")) + // { + // yError() << "[OpenXRModule::configure] Unable to open the port " << portName; + // return false; + // } + + // if (!YarpHelper::getStringFromSearchable(rf, "playerOrientationPort", portName)) + // { + // yError() << "[OpenXRModule::configure] Unable to get a string from a searchable"; + // return false; + // } + // if (!m_playerOrientationPort.open("/" + getName() + portName)) + // { + // yError() << "[OpenXRModule::configure] Unable to open the port " << portName; + // return false; + // } + + if (!YarpHelper::getStringFromSearchable(rf, "rpcWalkingPort_name", portName)) + { + yError() << "[OpenXRModule::configure] Unable to get a string from a searchable"; + return false; + } + if (!m_pImpl->rpcWalkingClient.open("/" + getName() + portName)) + { + yError() << "[OpenXRModule::configure] " << portName << " port already open."; + return false; + } + + // if (!YarpHelper::getStringFromSearchable(rf, "rpcVirtualizerPort_name", portName)) + // { + // yError() << "[OpenXRModule::configure] Unable to get a string from a searchable"; + // return false; + // } + // if (!m_rpcVirtualizerClient.open("/" + getName() + portName)) + // { + // yError() << "[OpenXRModule::configure] " << portName << " port already open."; + // return false; + // } + + m_pImpl->playerOrientation = 0; + // m_playerOrientationOld = 0; + // m_robotYaw = 0; + + m_pImpl->openXRHeadsetPoseInertial.resize(6, 0.0); + + // Reset the cameras if necessary + bool resetCameras = generalOptions.check("resetCameras", yarp::os::Value(false)).asBool(); + yInfo() << "[OpenXRModule::configure] Reset camera: " << resetCameras; + if (resetCameras) + { + + std::string leftCameraPort, rightCameraPort; + if (!YarpHelper::getStringFromSearchable(generalOptions, "leftCameraPort", leftCameraPort)) + { + yError() << "[OpenXRModule::configure] resetCameras is true, but leftCameraPort is not " + "provided."; + return false; + } + + if (!YarpHelper::getStringFromSearchable( + generalOptions, "rightCameraPort", rightCameraPort)) + { + yError() << "[OpenXRModule::configure] resetCameras is true, but rightCameraPort is " + "not provided."; + return false; + } + + if (!m_pImpl->resetCamera(leftCameraPort, "/walking-teleoperation/camera-reset/left")) + { + yError() << "[OpenXRModule::configure] Failed to reset left camera."; + return false; + } + + if (!m_pImpl->resetCamera(rightCameraPort, "/walking-teleoperation/camera-reset/right")) + { + yError() << "[OpenXRModule::configure] Failed to reset right camera."; + return false; + } + yInfo() << "[OpenXRModule::configure] Cameras have been reset."; + } + + m_pImpl->state = Impl::OpenXRFSM::Configured; + + return true; +} + +double OpenXRModule::getPeriod() +{ + return m_pImpl->dT; +} + +bool OpenXRModule::close() +{ + + m_pImpl->head->controlHelper()->close(); + + // if (!m_useSenseGlove) + // { + // m_rightHandFingers->controlHelper()->close(); + // m_leftHandFingers->controlHelper()->close(); + // } + + m_pImpl->joypadDevice.close(); + m_pImpl->transformClientDevice.close(); + + return true; +} + + +bool OpenXRModule::updateModule() +{ + if (!m_pImpl->getFeedbacks()) + { + yError() << "[OpenXRModule::updateModule] Unable to get the feedback"; + return false; + } + + m_pImpl->buttonsState = m_pImpl->getDeviceButtonsState(); + + if (m_pImpl->state == Impl::OpenXRFSM::Running) + { + + // get the transformation form the oculus + if (!m_pImpl->getTransforms()) + { + yError() << "[OpenXRModule::updateModule] Unable to get the transform"; + return false; + } + + m_pImpl->head->setPlayerOrientation(m_pImpl->playerOrientation); + m_pImpl->head->setDesiredHeadOrientationFromOpenXr(m_pImpl->openXRRoot_T_headOpenXR); + + if (m_pImpl->moveRobot) + { + if (!m_pImpl->head->move()) + { + yError() << "[updateModule::updateModule] unable to move the head"; + return false; + } + } + + // update left hand transformation values + yarp::sig::Vector& leftHandPose = m_pImpl->leftHandPosePort.prepare(); + m_pImpl->leftHand->setPlayerOrientation(m_pImpl->playerOrientation); + m_pImpl->leftHand->setHandTransform(m_pImpl->openXRRoot_T_lOpenXR); + + // update right hand transformation values + yarp::sig::Vector& rightHandPose = m_pImpl->rightHandPosePort.prepare(); + m_pImpl->rightHand->setPlayerOrientation(m_pImpl->playerOrientation); + m_pImpl->rightHand->setHandTransform(m_pImpl->openXRRoot_T_rOpenXR); + + m_pImpl->leftHand->evaluateDesiredHandPose(leftHandPose); + m_pImpl->rightHand->evaluateDesiredHandPose(rightHandPose); + + // move the robot + if (m_pImpl->moveRobot) + { + m_pImpl->leftHandPosePort.write(); + m_pImpl->rightHandPosePort.write(); + } + + if (m_pImpl->moveRobot) + { + // send commands to the walking + yarp::os::Bottle cmd, outcome; + double x{0.0}, y{0.0}; + + if (m_pImpl->isButtonStateEqualToMask(m_pImpl->joypadParameters.joypadButtonsMap)) + { + m_pImpl->joypadControllerInterface->getAxis(m_pImpl->joypadParameters.xJoypadIndex, x); + m_pImpl->joypadControllerInterface->getAxis(m_pImpl->joypadParameters.yJoypadIndex, y); + + x = -m_pImpl->joypadParameters.scaleX * m_pImpl->deadzone(x); + y = m_pImpl->joypadParameters.scaleY * m_pImpl->deadzone(y); + std::swap(x, y); + } + + // send commands to the walking + cmd.addString("setGoal"); + cmd.addDouble(x); + cmd.addDouble(y); + m_pImpl->rpcWalkingClient.write(cmd, outcome); + } + + // left fingers + const double leftFingersVelocity = m_pImpl->evaluateDesiredFingersVelocity( + m_pImpl->joypadParameters.leftFingersSqueezeButtonsMap, + m_pImpl->joypadParameters.leftFingersReleaseButtonsMap, + m_pImpl->joypadParameters.fingersVelocityLeftIndex); + + if (!m_pImpl->leftHandFingers->setFingersVelocity(leftFingersVelocity)) + { + yError() << "[OpenXRModule::updateModule] Unable to set the left finger velocity."; + return false; + } + if (m_pImpl->moveRobot) + { + if (!m_pImpl->leftHandFingers->move()) + { + yError() << "[OpenXRModule::updateModule] Unable to move the left finger"; + return false; + } + } + + const double rightFingersVelocity = m_pImpl->evaluateDesiredFingersVelocity( + m_pImpl->joypadParameters.rightFingersSqueezeButtonsMap, + m_pImpl->joypadParameters.rightFingersReleaseButtonsMap, + m_pImpl->joypadParameters.fingersVelocityRightIndex); + + if (!m_pImpl->rightHandFingers->setFingersVelocity(rightFingersVelocity)) + { + yError() << "[OpenXRModule::updateModule] Unable to set the right finger velocity."; + return false; + } + if (m_pImpl->moveRobot) + { + if (!m_pImpl->rightHandFingers->move()) + { + yError() << "[OpenXRModule::updateModule] Unable to move the right finger"; + return false; + } + } + + + + // check if it is time to stop walking + if (m_pImpl->isButtonStateEqualToMask(m_pImpl->joypadParameters.stopWalkingButtonsMap)) + { + if (m_pImpl->moveRobot) + { + yarp::os::Bottle cmd, outcome; + cmd.addString("stopWalking"); + m_pImpl->rpcWalkingClient.write(cmd, outcome); + } + yInfo() << "[OpenXRModule::updateModule] stop"; + return false; + } + + } else if (m_pImpl->state == Impl::OpenXRFSM::Configured) + { + // // check if it is time to prepare or start walking + if (m_pImpl->isButtonStateEqualToMask(m_pImpl->joypadParameters.prepareWalkingButtonsMap)) + { + // TODO add a visual feedback for the user + if (m_pImpl->moveRobot) + { + yarp::os::Bottle cmd, outcome; + cmd.addString("prepareRobot"); + m_pImpl->rpcWalkingClient.write(cmd, outcome); + } + m_pImpl->state = OpenXRModule::Impl::OpenXRFSM::InPreparation; + yInfo() << "[OpenXRModule::updateModule] prepare the robot"; + } + } else if (m_pImpl->state == Impl::OpenXRFSM::InPreparation) + { + if (m_pImpl->moveRobot) + { + m_pImpl->head->initializeNeckJointValues(); + if (!m_pImpl->head->move()) + { + yError() << "[OpenXRModule::updateModule] unable to move the head"; + return false; + } + } + + if (m_pImpl->isButtonStateEqualToMask(m_pImpl->joypadParameters.startWalkingButtonsMap)) + { + if (!m_pImpl->frameTransformInterface->frameExists(m_pImpl->headFrameName)) + { + yError() << "[OpenXRModule::updateModule] The frame named " + << m_pImpl->headFrameName << " does not exist."; + yError() << "[OpenXRModule::updateModule] I will not start the walking. Please " + "try to start again."; + return true; + } + + yarp::sig::Matrix openXrHeadInitialTransform = identitySE3(); + if (!m_pImpl->frameTransformInterface->getTransform( + m_pImpl->headFrameName, m_pImpl->rootFrameName, openXrHeadInitialTransform)) + { + yError() << "[OpenXRModule::updateModule] Unable to evaluate the " + << m_pImpl->headFrameName << " to " << m_pImpl->rootFrameName + << "transformation"; + yError() << "[OpenXRModule::updateModule] I will not start the walking. Please " + "try to start again."; + return true; + } + + // get only the yaw axis + iDynTree::Rotation tempRot; + iDynTree::toEigen(tempRot) = getRotation(openXrHeadInitialTransform); + + double r, p, y; + + // This code was taken from https://www.geometrictools.com/Documentation/EulerAngles.pdf + // Section 2.2 + auto inverseKinematicsXZY = [](const iDynTree::Rotation &chest_R_head, + double &neckPitch, + double &neckRoll, + double &neckYaw) + { + if (chest_R_head(0,1) < +1.0) + { + if (chest_R_head(0, 1) > -1.0) + { + neckRoll = std::asin(-chest_R_head(0, 1)); //The roll is thetaZ + neckPitch = std::atan2(chest_R_head(2, 1), chest_R_head(1, 1)); //The pitch is thetaX + neckYaw = std::atan2(chest_R_head(0, 2), chest_R_head(0, 0)); //The yaw is thetay + } + else + { + neckRoll = M_PI/2.0; + neckPitch = -std::atan2(-chest_R_head(2, 0), chest_R_head(2, 2)); + neckYaw = 0.0; + } + } + else + { + neckRoll = -M_PI/2.0; + neckPitch = std::atan2(-chest_R_head(2, 0), chest_R_head(2, 2)); + neckYaw = 0.0; + } + + }; + + inverseKinematicsXZY(tempRot, p,r,y); + + + iDynTree::Transform tempTransform; + tempTransform.setRotation(iDynTree::Rotation::RotY(y)); // We remove only the initial rotation of the person head around gravity. + + iDynTree::Position tmpPos; + iDynTree::toEigen(tmpPos) = getPosition(openXrHeadInitialTransform); + tempTransform.setPosition(tmpPos); + +// std::cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> Posizione da root a head --> " << std::endl << iDynTree::toEigen(openXrHeadInitialTransform).col(3) << std::endl; +// std::cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> getPosition --> " << std::endl << getPosition(openXrHeadInitialTransform) << std::endl; +// std::cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> tmpPos --> " << std::endl << iDynTree::toEigen(tmpPos) << std::endl; + + iDynTree::toEigen(m_pImpl->openXRInitialAlignement) + = iDynTree::toEigen(tempTransform.inverse().asHomogeneousTransform()); + +// std::cout << "------------------------------- Initial alignment m_pImpl->openXRInitialAlignement --> " << std::endl << iDynTree::toEigen(m_pImpl->openXRInitialAlignement); +// std::cout << "------------------------------- Initial alignment position --> " << std::endl << iDynTree::toEigen(m_pImpl->openXRInitialAlignement).col(3); + + +// yInfo() << "------------------------------- Initial alignment (deg): roll=" << iDynTree::rad2deg(r) << " pitch=" << iDynTree::rad2deg(p) << " yaw=" << iDynTree::rad2deg(y); + + if (m_pImpl->moveRobot) + { + yarp::os::Bottle cmd, outcome; + cmd.addString("startWalking"); + m_pImpl->rpcWalkingClient.write(cmd, outcome); + } + + // if(outcome.get(0).asBool()) + m_pImpl->state = OpenXRModule::Impl::OpenXRFSM::Running; + yInfo() << "[OpenXRModule::updateModule] start the robot"; + yInfo() << "[OpenXRModule::updateModule] Running ..."; + } + } + return true; +} diff --git a/modules/OpenXR_module/src/main.cpp b/modules/OpenXR_module/src/main.cpp new file mode 100644 index 00000000..116dd521 --- /dev/null +++ b/modules/OpenXR_module/src/main.cpp @@ -0,0 +1,37 @@ +/** + * @file main.cpp + * @authors Giulio Romualdi + * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2018 + */ + +// YARP +#include +#include +#include + +#include + +int main(int argc, char* argv[]) +{ + // initialise yarp network + yarp::os::Network yarp; + if (!yarp.checkNetwork()) + { + yError() << "[main] Unable to find YARP network"; + return EXIT_FAILURE; + } + + // prepare and configure the resource finder + yarp::os::ResourceFinder& rf = yarp::os::ResourceFinder::getResourceFinderSingleton(); + + rf.setDefaultConfigFile("openXRConfig.ini"); + + rf.configure(argc, argv); + + // create the module + OpenXRModule module; + + return module.runModule(rf); +} diff --git a/modules/Utils/include/Utils.hpp b/modules/Utils/include/Utils.hpp index 010c7573..819a706e 100644 --- a/modules/Utils/include/Utils.hpp +++ b/modules/Utils/include/Utils.hpp @@ -133,6 +133,17 @@ bool getYarpVectorFromSearchable(const yarp::os::Searchable& config, const std::string& key, yarp::sig::Vector& vector); +/** + * Extract an int vector from a searchable object. + * @param config is the searchable object; + * @param key the name to check for; + * @param vector is the vector. + * @return true/false in case of success/failure + */ +bool getIntVectorFromSearchable(const yarp::os::Searchable& config, + const std::string& key, + std::vector& output); + /** * Merge two vectors. vector = [vector, t] * @param vector the original vector. The new elements will be add at the end of diff --git a/modules/Utils/src/Utils.cpp b/modules/Utils/src/Utils.cpp index 041532cc..210bfc61 100644 --- a/modules/Utils/src/Utils.cpp +++ b/modules/Utils/src/Utils.cpp @@ -142,6 +142,39 @@ bool YarpHelper::getYarpVectorFromSearchable(const yarp::os::Searchable& config, return true; } +bool YarpHelper::getIntVectorFromSearchable(const yarp::os::Searchable& config, + const std::string& key, + std::vector& output) +{ + yarp::os::Value* value; + if (!config.check(key, value)) + { + yError() << "[getIntVectorFromSearchable] Missing field " << key; + return false; + } + + if (!value->isList()) + { + yError() << "[getIntVectorFromSearchable] the value is not a double."; + return false; + } + + yarp::os::Bottle* inputPtr = value->asList(); + + output.resize(inputPtr->size()); + + for (int i = 0; i < inputPtr->size(); i++) + { + if (!inputPtr->get(i).isInt()) + { + yError() << "[getIntVectorFromSearchable] The input is expected to be an int"; + return false; + } + output[i] = inputPtr->get(i).asInt(); + } + return true; +} + void YarpHelper::populateBottleWithStrings(yarp::os::Bottle& bottle, const std::initializer_list& strings) {