From 8a0d50fd14ecc23f02855f59c680ac8bf6e262cc Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 29 Sep 2021 11:10:50 +0200 Subject: [PATCH 01/18] Move all the classes required by the oculus in a separate library --- modules/CMakeLists.txt | 1 + modules/Common/CMakeLists.txt | 46 +++++++++++++++++++ .../include/FingersRetargeting.hpp | 0 .../include/HandRetargeting.hpp | 0 .../include/HeadRetargeting.hpp | 0 .../include/RetargetingController.hpp | 0 .../include/RobotControlHelper.hpp | 0 .../src/FingersRetargeting.cpp | 0 .../src/HandRetargeting.cpp | 0 .../src/HeadRetargeting.cpp | 0 .../src/RetargetingController.cpp | 0 .../src/RobotControlHelper.cpp | 0 modules/Oculus_module/CMakeLists.txt | 19 ++------ 13 files changed, 52 insertions(+), 14 deletions(-) create mode 100644 modules/Common/CMakeLists.txt rename modules/{Oculus_module => Common}/include/FingersRetargeting.hpp (100%) rename modules/{Oculus_module => Common}/include/HandRetargeting.hpp (100%) rename modules/{Oculus_module => Common}/include/HeadRetargeting.hpp (100%) rename modules/{Oculus_module => Common}/include/RetargetingController.hpp (100%) rename modules/{Oculus_module => Common}/include/RobotControlHelper.hpp (100%) rename modules/{Oculus_module => Common}/src/FingersRetargeting.cpp (100%) rename modules/{Oculus_module => Common}/src/HandRetargeting.cpp (100%) rename modules/{Oculus_module => Common}/src/HeadRetargeting.cpp (100%) rename modules/{Oculus_module => Common}/src/RetargetingController.cpp (100%) rename modules/{Oculus_module => Common}/src/RobotControlHelper.cpp (100%) diff --git a/modules/CMakeLists.txt b/modules/CMakeLists.txt index a7b9fcf9f..22e9bdf9a 100644 --- a/modules/CMakeLists.txt +++ b/modules/CMakeLists.txt @@ -3,6 +3,7 @@ # Authors: Giulio Romualdi add_subdirectory(Utils) +add_subdirectory(Common) add_subdirectory(Oculus_module) if(WALKING_TELEOPERATION_COMPILE_XsensModule) diff --git a/modules/Common/CMakeLists.txt b/modules/Common/CMakeLists.txt new file mode 100644 index 000000000..3c13435b5 --- /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 669b899c0..d500d373c 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() From 94220aa8253b910d373e2dc57623f75948233f68 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 29 Sep 2021 13:15:13 +0200 Subject: [PATCH 02/18] Add the first draft of the OpenXR application --- modules/CMakeLists.txt | 1 + modules/OpenXR_module/CMakeLists.txt | 63 ++ .../OpenXR_module/include/OpenXRModule.hpp | 240 +++++ modules/OpenXR_module/src/OpenXRModule.cpp | 931 ++++++++++++++++++ modules/OpenXR_module/src/main.cpp | 37 + 5 files changed, 1272 insertions(+) create mode 100644 modules/OpenXR_module/CMakeLists.txt create mode 100644 modules/OpenXR_module/include/OpenXRModule.hpp create mode 100644 modules/OpenXR_module/src/OpenXRModule.cpp create mode 100644 modules/OpenXR_module/src/main.cpp diff --git a/modules/CMakeLists.txt b/modules/CMakeLists.txt index 22e9bdf9a..86f4e0fef 100644 --- a/modules/CMakeLists.txt +++ b/modules/CMakeLists.txt @@ -5,6 +5,7 @@ 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/OpenXR_module/CMakeLists.txt b/modules/OpenXR_module/CMakeLists.txt new file mode 100644 index 000000000..88f66b33e --- /dev/null +++ b/modules/OpenXR_module/CMakeLists.txt @@ -0,0 +1,63 @@ +# 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) diff --git a/modules/OpenXR_module/include/OpenXRModule.hpp b/modules/OpenXR_module/include/OpenXRModule.hpp new file mode 100644 index 000000000..84d3a5902 --- /dev/null +++ b/modules/OpenXR_module/include/OpenXRModule.hpp @@ -0,0 +1,240 @@ +/** + * @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; + +// double m_dT; /**< Module period. */ + +// /** Oculus Finite state machine */ +// enum class OculusFSM +// { +// Configured, +// Running, +// InPreparation +// }; + +// OculusFSM m_state; /**< State of the OculusFSM */ + +// // joypad utils +// // TODO move in a separate class +// double m_deadzone; /**< Joypad deadzone */ +// double m_fullscale; /**< Joypad fullscale */ +// double m_scaleX; /**< Scaling factor on the x axis */ +// double m_scaleY; /**< Scaling factor on the y axis */ +// double m_joypadX; /**< x value */ +// double m_joypadY; /**< y value */ + +// unsigned int m_xJoypadIndex; /**< Mapping of the axis related to x coordinate */ +// unsigned int m_yJoypadIndex; /**< Mapping of the axis related to y coordinate */ + +// unsigned int m_squeezeLeftIndex; /**< Index of the trigger used for squeezing the left hand */ +// unsigned int m_squeezeRightIndex; /**< Index of the trigger used for squeezing the right hand */ + +// unsigned int m_releaseLeftIndex; /**< Index of the trigger used for releasing the left hand */ +// unsigned int m_releaseRightIndex; /**< Index of the trigger used for releasing the right hand */ + +// unsigned int m_startWalkingIndex; /**< Index of the start walking button */ +// unsigned int m_stopWalkingIndex; /**< Index of the stop walking button */ +// unsigned int m_prepareWalkingIndex; /**< Index of the prepare walking button */ + +// bool m_useVirtualizer; /**< True if the virtualizer is used in the retargeting */ +// bool m_useXsens; /**< True if the Xsens is used in the retargeting */ +// bool m_useIFeel; /**< True if the iFeel is used in the retargeting */ +// bool m_useOpenXr; /**< True if OpenXr is used in the retargeting */ +// bool m_useSenseGlove; /**< True if the SenseGlove is used in the retargeting */ +// bool m_moveRobot; /**< the option to give the user the possibility to do not move the robot +// (default :: true)*/ + +// // transform server +// yarp::dev::PolyDriver m_transformClientDevice; /**< Transform client. */ +// yarp::dev::IFrameTransform* m_frameTransformInterface{nullptr}; /**< Frame transform +// interface. */ +// std::string m_rootFrameName; /**< Name of the root frame used in the transform server */ +// std::string m_headFrameName; /**< Name of the head frame used in the transform server (NOT +// SUPPORTED BY YARP)*/ +// std::string +// m_leftHandFrameName; /**< Name of the left hand frame used in the transform server */ +// std::string +// m_rightHandFrameName; /**< Name of the right hand frame used in the transform server */ + +// yarp::dev::PolyDriver m_joypadDevice; /**< Joypad polydriver. */ +// yarp::dev::IJoypadController* m_joypadControllerInterface{nullptr}; /**< joypad interface. */ + +// std::unique_ptr m_head; /**< Pointer to the head retargeting object. */ +// std::unique_ptr m_leftHandFingers; /**< Pointer to the left +// finger retargeting object. */ +// std::unique_ptr m_rightHandFingers; /**< Pointer to the right +// finger retargeting object. */ +// std::unique_ptr m_rightHand; /**< Pointer to the right +// hand retargeting object. */ +// std::unique_ptr m_leftHand; /**< Pointer to the left hand +// retargeting object. */ + +// // ports +// yarp::os::BufferedPort m_leftHandPosePort; /**< Left hand port pose. */ +// yarp::os::BufferedPort m_rightHandPosePort; /**< Right hand port pose. */ + +// /** Player orientation port. */ +// yarp::os::BufferedPort m_playerOrientationPort; +// /** Port used to simulate an imu for moving the images. */ +// yarp::os::BufferedPort m_imagesOrientationPort; +// /** Port used to retrieve the robot base orientation. */ +// yarp::os::BufferedPort m_robotOrientationPort; +// /** Port used to retrieve the headset oculus orientation. */ +// yarp::os::BufferedPort m_oculusOrientationPort; +// /** Port used to retrieve the headset oculus position. */ +// yarp::os::BufferedPort m_oculusPositionPort; + +// yarp::os::RpcClient m_rpcWalkingClient; /**< Rpc client used for sending command to the walking +// controller */ +// yarp::os::RpcClient +// m_rpcVirtualizerClient; /**< Rpc client used for sending command to the virtualizer */ + +// /** Port used to retrieve the human whole body joint pose. */ +// yarp::os::BufferedPort m_wholeBodyHumanJointsPort; + +// /** Port used to retrieve the human whole body joint pose. */ +// yarp::os::BufferedPort m_wholeBodyHumanSmoothedJointsPort; + +// double m_robotYaw; /**< Yaw angle of the robot base */ + +// yarp::sig::Matrix m_oculusRoot_T_lOculus; +// yarp::sig::Matrix m_oculusRoot_T_rOculus; +// yarp::sig::Matrix m_oculusRoot_T_headOculus; +// yarp::sig::Matrix m_openXrInitialAlignement; + +// std::vector m_oculusHeadsetPoseInertial; + +// double m_playerOrientation{0}; /**< Player orientation (read by the Virtualizer) +// only yaw. */ +// double m_playerOrientationOld{0}; /**< previous updated Player orientation (read by the +// Virtualizer) only yaw. */ +// double m_playerOrientationThreshold; /**< Player orientation threshold. */ + +// bool m_enableLogger; /**< log the data (if ON) */ +// #ifdef ENABLE_LOGGER +// XBot::MatLogger2::Ptr m_logger; /**< */ +// XBot::MatAppender::Ptr m_appender; +// std::string m_logger_prefix{"oculus"}; +// #endif +// /** +// * Configure the Oculus. +// * @param config configuration object +// * @return true in case of success and false otherwise. +// */ +// bool configureOculus(const yarp::os::Searchable& config); + +// /** +// * Configure the Tranformation Client. +// * @param config configuration object +// * @return true in case of success and false otherwise. +// */ +// bool configureTranformClient(const yarp::os::Searchable& config); + +// /** +// * Configure the Joypad. +// * @param config configuration object +// * @return true in case of success and false otherwise. +// */ +// bool configureJoypad(const yarp::os::Searchable& config); + +// /** +// * @brief Reset a robot camera to its default settings. +// * @param cameraPort The remote port to the camera +// * @param localPort The local port needed for the driver to open +// * @return true in case of success and false otherwise. +// */ +// bool resetCamera(const std::string& cameraPort, const std::string& localPort); + +// /** +// * Get all the feedback signal from the interfaces +// * @return true in case of success and false otherwise. +// */ +// bool getFeedbacks(); + +// /** +// * Implements a deadzone for the joypad +// * @return joypad inputs after applying deadzone. +// */ +// double deadzone(const double&); + +// /** +// * Evaluate the desired fingers velocity +// * @param squeezeIndex index used for squeezing +// * @param releaseIndex index used for releasing +// * @return the the desired joints velocity for the fingers in radiant / seconds +// */ +// double evaluateDesiredFingersVelocity(unsigned int squeezeIndex, unsigned int releaseIndex); + +// /** +// * Get the transformation from the transform server +// * @return true in case of success and false otherwise. +// */ +// bool getTransforms(); + +// /** +// * Open the logger +// * @return true if it could open the logger +// */ +// bool openLogger(); + +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 000000000..e6df1f86d --- /dev/null +++ b/modules/OpenXR_module/src/OpenXRModule.cpp @@ -0,0 +1,931 @@ +/** + * @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 + */ + +// 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 */ + }; + 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. */ + + // 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 (NOT + SUPPORTED BY YARP)*/ + 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. */ + + 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 (!m_useXsens && !m_useIFeel) + // { + + // if (!m_frameTransformInterface->frameExists(m_leftHandFrameName)) + // { + + // yError() << "[OpenXRModule::getTransforms] No " << m_leftHandFrameName << " + // frame."; return false; + // } + + // if (!m_frameTransformInterface->frameExists(m_rightHandFrameName)) + // { + // yError() << "[OpenXRModule::getTransforms] No " << m_rightHandFrameName + // << " frame."; + // return false; + // } + + // if (!m_frameTransformInterface->getTransform( + // m_leftHandFrameName, m_rootFrameName, m_oculusRoot_T_lOculus)) + // { + // yError() << "[OpenXRModule::getTransforms] Unable to evaluate the " + // << m_leftHandFrameName << " to " << m_rootFrameName << "transformation"; + // return false; + // } + + // if (!m_frameTransformInterface->getTransform( + // m_rightHandFrameName, m_rootFrameName, m_oculusRoot_T_rOculus)) + // { + // yError() << "[OpenXRModule::getTransforms] Unable to evaluate the " + // << m_rightHandFrameName << " to " << m_rootFrameName << + // "transformation"; + // return false; + // } + // } + 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; + } + + + // TODO set index of joypad + // set the index of the axis according to the OVRheadset yarp device + // bool useLeftStick = config.check("use_left", yarp::os::Value("false")).asBool(); + // m_xJoypadIndex = useLeftStick ? 4 : 6; + // m_yJoypadIndex = useLeftStick ? 5 : 7; + + // m_squeezeLeftIndex = 0; + // m_squeezeRightIndex = 1; + + // m_releaseLeftIndex = 2; + // m_releaseRightIndex = 3; + + // m_startWalkingIndex = 4; // X button + // m_stopWalkingIndex = 5; // X button + // m_prepareWalkingIndex = 0; // A button + + 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; + } + + 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; + }; +}; + +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; + } + + // if (!m_useSenseGlove) + // { + // // configure fingers retargeting + // m_leftHandFingers = std::make_unique(); + // yarp::os::Bottle& leftFingersOptions = rf.findGroup("LEFT_FINGERS_RETARGETING"); + // leftFingersOptions.append(generalOptions); + // if (!m_leftHandFingers->configure(leftFingersOptions, getName())) + // { + // yError() + // << "[OpenXRModule::configure] Unable to initialize the left fingers retargeting."; + // return false; + // } + + // m_rightHandFingers = std::make_unique(); + // yarp::os::Bottle& rightFingersOptions = rf.findGroup("RIGHT_FINGERS_RETARGETING"); + // rightFingersOptions.append(generalOptions); + // if (!m_rightHandFingers->configure(rightFingersOptions, getName())) + // { + // yError() + // << "[OpenXRModule::configure] Unable to initialize the right fingers retargeting."; + // return false; + // } + // } + + // configure hands retargeting + // m_leftHand = std::make_unique(); + // yarp::os::Bottle& leftHandOptions = rf.findGroup("LEFT_HAND_RETARGETING"); + // leftHandOptions.append(generalOptions); + // if (!m_leftHand->configure(leftHandOptions)) + // { + // yError() << "[OpenXRModule::configure] Unable to initialize the left fingers retargeting."; + // return false; + // } + + // m_rightHand = std::make_unique(); + // yarp::os::Bottle& rightHandOptions = rf.findGroup("RIGHT_HAND_RETARGETING"); + // rightHandOptions.append(generalOptions); + // if (!m_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_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_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_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; +} + +// double OpenXRModule::evaluateDesiredFingersVelocity(unsigned int squeezeIndex, +// unsigned int releaseIndex) +// { +// double releaseFingersVelocity, squeezeFingersVelocity; +// m_joypadControllerInterface->getAxis(squeezeIndex, squeezeFingersVelocity); +// m_joypadControllerInterface->getAxis(releaseIndex, releaseFingersVelocity); + +// if (squeezeFingersVelocity > releaseFingersVelocity) +// return squeezeFingersVelocity; +// else if (squeezeFingersVelocity < releaseFingersVelocity) +// return -releaseFingersVelocity; +// else +// return 0; +// } + +bool OpenXRModule::updateModule() +{ + if (!m_pImpl->getFeedbacks()) + { + yError() << "[OpenXRModule::updateModule] Unable to get the feedback"; + return false; + } + + 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); + + // m_head->setDesiredHeadOrientation(desiredHeadOrientationVector(0), + // desiredHeadOrientationVector(1), desiredHeadOrientationVector(2)); + if (m_pImpl->moveRobot) + { + if (!m_pImpl->head->move()) + { + yError() << "[updateModule::updateModule] unable to move the head"; + return false; + } + } + // if (!m_useXsens && !m_useIFeel) + // { + // // update left hand transformation values + // yarp::sig::Vector& leftHandPose = m_leftHandPosePort.prepare(); + // m_leftHand->setPlayerOrientation(m_playerOrientation); + // m_leftHand->setHandTransform(m_oculusRoot_T_lOculus); + + // // update right hand transformation values + // yarp::sig::Vector& rightHandPose = m_rightHandPosePort.prepare(); + // m_rightHand->setPlayerOrientation(m_playerOrientation); + // m_rightHand->setHandTransform(m_oculusRoot_T_rOculus); + + // if (m_useVirtualizer) + // { + // if (std::abs(m_playerOrientation - m_playerOrientationOld) + // > m_playerOrientationThreshold) + // { + // iDynTree::Position teleopPosition = {m_oculusHeadsetPoseInertial[0], + // m_oculusHeadsetPoseInertial[1], + // m_oculusHeadsetPoseInertial[2]}; + + // m_leftHand->setPlayerPosition(teleopPosition); + // m_rightHand->setPlayerPosition(teleopPosition); + // m_playerOrientationOld = m_playerOrientation; + // } + // } + + // // evaluate the robot hands' pose + // m_leftHand->evaluateDesiredHandPose(leftHandPose); + // m_rightHand->evaluateDesiredHandPose(rightHandPose); + + // // move the robot + // if (m_moveRobot) + // { + // m_leftHandPosePort.write(); + // m_rightHandPosePort.write(); + // } + // } + + // use joypad + // std::vector locCmd; + // if (!m_useVirtualizer) + // { + // yarp::os::Bottle cmd, outcome; + // double x, y; + // m_joypadControllerInterface->getAxis(m_xJoypadIndex, x); + // m_joypadControllerInterface->getAxis(m_yJoypadIndex, y); + + // x = -m_scaleX * deadzone(x); + // y = m_scaleY * deadzone(y); + // std::swap(x, y); + + // cmd.addString("setGoal"); + // cmd.addDouble(x); + // cmd.addDouble(y); + // if (m_moveRobot) + // { + // m_rpcWalkingClient.write(cmd, outcome); + // } + // locCmd.push_back(x); + // locCmd.push_back(y); + // } + + // if (!m_useSenseGlove) + // { + // // left fingers + // double leftFingersVelocity + // = evaluateDesiredFingersVelocity(m_squeezeLeftIndex, m_releaseLeftIndex); + // if (!m_leftHandFingers->setFingersVelocity(leftFingersVelocity)) + // { + // yError() << "[OpenXRModule::updateModule] Unable to set the left finger + // velocity."; return false; + // } + // if (m_moveRobot) + // { + // if (!m_leftHandFingers->move()) + // { + // yError() << "[OpenXRModule::updateModule] Unable to move the left finger"; + // return false; + // } + // } + + // // right fingers + // double rightFingersVelocity + // = evaluateDesiredFingersVelocity(m_squeezeRightIndex, m_releaseRightIndex); + // if (!m_rightHandFingers->setFingersVelocity(rightFingersVelocity)) + // { + // yError() << "[OpenXRModule::updateModule] Unable to set the right finger + // velocity."; return false; + // } + // if (m_moveRobot) + // { + // if (!m_rightHandFingers->move()) + // { + // yError() << "[OpenXRModule::updateModule] Unable to move the right finger"; + // return false; + // } + // } + // } + + // // check if it is time to prepare or start walking + // float buttonMapping; + + // // prepare robot (A button) + // m_joypadControllerInterface->getButton(m_stopWalkingIndex, buttonMapping); + // yarp::os::Bottle cmd, outcome; + + // if (buttonMapping > 0) + // { + // // TODO add a visual feedback for the user + // if (m_moveRobot) + // { + // cmd.addString("stopWalking"); + // m_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 + // float buttonMapping; + + // // prepare robot (A button) + // m_joypadControllerInterface->getButton(m_prepareWalkingIndex, buttonMapping); + // yarp::os::Bottle cmd, outcome; + + // if (buttonMapping > 0) + // { + // // TODO add a visual feedback for the user + // if (m_moveRobot) + // { + // cmd.addString("prepareRobot"); + // m_rpcWalkingClient.write(cmd, outcome); + // } + // m_state = OculusFSM::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; + } + } + + // TODO reenable the initial pose resetting + // float buttonMapping; + // start walking (X button) + + + // m_joypadControllerInterface->getButton(m_startWalkingIndex, buttonMapping); + // yarp::os::Bottle cmd, outcome; + + // if (buttonMapping > 0) + // { + // if (m_useOpenXr) + // { + // if (!m_frameTransformInterface->frameExists(m_headFrameName)) + // { + // yError() << "[OpenXRModule::updateModule] The frame named " << m_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_frameTransformInterface->getTransform( + // m_headFrameName, m_rootFrameName, openXrHeadInitialTransform)) + // { + // yError() << "[OpenXRModule::updateModule] Unable to evaluate the " + // << m_headFrameName << " to " << m_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 yaw = 0; + // double dummy = 0; + // tempRot.getRPY(dummy, yaw, dummy); + + // iDynTree::Transform tempTransform; + // tempTransform.setRotation(iDynTree::Rotation::RotY( + // yaw)); // We remove only the initial rotation of the person head around gravity. + // tempTransform.setPosition(iDynTree::make_span(getPosition( + // openXrHeadInitialTransform))); // We remove the initial position between the + // // head and the reference frame. + + // iDynTree::toEigen(m_openXrInitialAlignement) + // = iDynTree::toEigen(tempTransform.inverse().asHomogeneousTransform()); + // } + + // if (m_useVirtualizer) + // { + // // not sure if here causes the problem of hand rotation, check it + // // reset the player orientation of the virtualizer + // cmd.addString("resetPlayerOrientation"); + // m_rpcVirtualizerClient.write(cmd, outcome); + // cmd.clear(); + // } + + // // TODO add a visual feedback for the user + // if (m_moveRobot) + // { + // cmd.addString("startWalking"); + // m_rpcWalkingClient.write(cmd, outcome); + // } + + // // if(outcome.get(0).asBool()) + // m_state = OculusFSM::Running; + // yInfo() << "[OpenXRModule::updateModule] start the robot"; + // yInfo() << "[OpenXRModule::updateModule] Running ..."; + // } + } + + return true; +} + +// double OpenXRModule::deadzone(const double& input) +// { +// if (input >= 0) +// { +// if (input > m_deadzone) +// return (input - m_deadzone) / (m_fullscale - m_deadzone); +// else +// return 0.0; +// } else +// { +// if (input < -m_deadzone) +// return (input + m_deadzone) / (m_fullscale - m_deadzone); +// else +// return 0.0; +// } +// } diff --git a/modules/OpenXR_module/src/main.cpp b/modules/OpenXR_module/src/main.cpp new file mode 100644 index 000000000..116dd5213 --- /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); +} From 0b5c769f81630ac99527ee68ab8cae394627bb77 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 29 Sep 2021 19:44:06 +0200 Subject: [PATCH 03/18] Implement YarpHelper::getIntVectorFromSearchable() --- modules/Utils/include/Utils.hpp | 11 +++++++++++ modules/Utils/src/Utils.cpp | 33 +++++++++++++++++++++++++++++++++ 2 files changed, 44 insertions(+) diff --git a/modules/Utils/include/Utils.hpp b/modules/Utils/include/Utils.hpp index 010c7573e..819a706e2 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 041532cc6..210bfc61f 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) { From 8c58dc1ebe4be8032d3763c624a8f5b26268569c Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 29 Sep 2021 19:44:39 +0200 Subject: [PATCH 04/18] Finalize the first implementation of OpenXRModule --- .../OpenXR_module/include/OpenXRModule.hpp | 175 ---- modules/OpenXR_module/src/OpenXRModule.cpp | 981 +++++++++++------- 2 files changed, 621 insertions(+), 535 deletions(-) diff --git a/modules/OpenXR_module/include/OpenXRModule.hpp b/modules/OpenXR_module/include/OpenXRModule.hpp index 84d3a5902..4fa07b9e5 100644 --- a/modules/OpenXR_module/include/OpenXRModule.hpp +++ b/modules/OpenXR_module/include/OpenXRModule.hpp @@ -33,181 +33,6 @@ class OpenXRModule : public yarp::os::RFModule struct Impl; std::unique_ptr m_pImpl; -// double m_dT; /**< Module period. */ - -// /** Oculus Finite state machine */ -// enum class OculusFSM -// { -// Configured, -// Running, -// InPreparation -// }; - -// OculusFSM m_state; /**< State of the OculusFSM */ - -// // joypad utils -// // TODO move in a separate class -// double m_deadzone; /**< Joypad deadzone */ -// double m_fullscale; /**< Joypad fullscale */ -// double m_scaleX; /**< Scaling factor on the x axis */ -// double m_scaleY; /**< Scaling factor on the y axis */ -// double m_joypadX; /**< x value */ -// double m_joypadY; /**< y value */ - -// unsigned int m_xJoypadIndex; /**< Mapping of the axis related to x coordinate */ -// unsigned int m_yJoypadIndex; /**< Mapping of the axis related to y coordinate */ - -// unsigned int m_squeezeLeftIndex; /**< Index of the trigger used for squeezing the left hand */ -// unsigned int m_squeezeRightIndex; /**< Index of the trigger used for squeezing the right hand */ - -// unsigned int m_releaseLeftIndex; /**< Index of the trigger used for releasing the left hand */ -// unsigned int m_releaseRightIndex; /**< Index of the trigger used for releasing the right hand */ - -// unsigned int m_startWalkingIndex; /**< Index of the start walking button */ -// unsigned int m_stopWalkingIndex; /**< Index of the stop walking button */ -// unsigned int m_prepareWalkingIndex; /**< Index of the prepare walking button */ - -// bool m_useVirtualizer; /**< True if the virtualizer is used in the retargeting */ -// bool m_useXsens; /**< True if the Xsens is used in the retargeting */ -// bool m_useIFeel; /**< True if the iFeel is used in the retargeting */ -// bool m_useOpenXr; /**< True if OpenXr is used in the retargeting */ -// bool m_useSenseGlove; /**< True if the SenseGlove is used in the retargeting */ -// bool m_moveRobot; /**< the option to give the user the possibility to do not move the robot -// (default :: true)*/ - -// // transform server -// yarp::dev::PolyDriver m_transformClientDevice; /**< Transform client. */ -// yarp::dev::IFrameTransform* m_frameTransformInterface{nullptr}; /**< Frame transform -// interface. */ -// std::string m_rootFrameName; /**< Name of the root frame used in the transform server */ -// std::string m_headFrameName; /**< Name of the head frame used in the transform server (NOT -// SUPPORTED BY YARP)*/ -// std::string -// m_leftHandFrameName; /**< Name of the left hand frame used in the transform server */ -// std::string -// m_rightHandFrameName; /**< Name of the right hand frame used in the transform server */ - -// yarp::dev::PolyDriver m_joypadDevice; /**< Joypad polydriver. */ -// yarp::dev::IJoypadController* m_joypadControllerInterface{nullptr}; /**< joypad interface. */ - -// std::unique_ptr m_head; /**< Pointer to the head retargeting object. */ -// std::unique_ptr m_leftHandFingers; /**< Pointer to the left -// finger retargeting object. */ -// std::unique_ptr m_rightHandFingers; /**< Pointer to the right -// finger retargeting object. */ -// std::unique_ptr m_rightHand; /**< Pointer to the right -// hand retargeting object. */ -// std::unique_ptr m_leftHand; /**< Pointer to the left hand -// retargeting object. */ - -// // ports -// yarp::os::BufferedPort m_leftHandPosePort; /**< Left hand port pose. */ -// yarp::os::BufferedPort m_rightHandPosePort; /**< Right hand port pose. */ - -// /** Player orientation port. */ -// yarp::os::BufferedPort m_playerOrientationPort; -// /** Port used to simulate an imu for moving the images. */ -// yarp::os::BufferedPort m_imagesOrientationPort; -// /** Port used to retrieve the robot base orientation. */ -// yarp::os::BufferedPort m_robotOrientationPort; -// /** Port used to retrieve the headset oculus orientation. */ -// yarp::os::BufferedPort m_oculusOrientationPort; -// /** Port used to retrieve the headset oculus position. */ -// yarp::os::BufferedPort m_oculusPositionPort; - -// yarp::os::RpcClient m_rpcWalkingClient; /**< Rpc client used for sending command to the walking -// controller */ -// yarp::os::RpcClient -// m_rpcVirtualizerClient; /**< Rpc client used for sending command to the virtualizer */ - -// /** Port used to retrieve the human whole body joint pose. */ -// yarp::os::BufferedPort m_wholeBodyHumanJointsPort; - -// /** Port used to retrieve the human whole body joint pose. */ -// yarp::os::BufferedPort m_wholeBodyHumanSmoothedJointsPort; - -// double m_robotYaw; /**< Yaw angle of the robot base */ - -// yarp::sig::Matrix m_oculusRoot_T_lOculus; -// yarp::sig::Matrix m_oculusRoot_T_rOculus; -// yarp::sig::Matrix m_oculusRoot_T_headOculus; -// yarp::sig::Matrix m_openXrInitialAlignement; - -// std::vector m_oculusHeadsetPoseInertial; - -// double m_playerOrientation{0}; /**< Player orientation (read by the Virtualizer) -// only yaw. */ -// double m_playerOrientationOld{0}; /**< previous updated Player orientation (read by the -// Virtualizer) only yaw. */ -// double m_playerOrientationThreshold; /**< Player orientation threshold. */ - -// bool m_enableLogger; /**< log the data (if ON) */ -// #ifdef ENABLE_LOGGER -// XBot::MatLogger2::Ptr m_logger; /**< */ -// XBot::MatAppender::Ptr m_appender; -// std::string m_logger_prefix{"oculus"}; -// #endif -// /** -// * Configure the Oculus. -// * @param config configuration object -// * @return true in case of success and false otherwise. -// */ -// bool configureOculus(const yarp::os::Searchable& config); - -// /** -// * Configure the Tranformation Client. -// * @param config configuration object -// * @return true in case of success and false otherwise. -// */ -// bool configureTranformClient(const yarp::os::Searchable& config); - -// /** -// * Configure the Joypad. -// * @param config configuration object -// * @return true in case of success and false otherwise. -// */ -// bool configureJoypad(const yarp::os::Searchable& config); - -// /** -// * @brief Reset a robot camera to its default settings. -// * @param cameraPort The remote port to the camera -// * @param localPort The local port needed for the driver to open -// * @return true in case of success and false otherwise. -// */ -// bool resetCamera(const std::string& cameraPort, const std::string& localPort); - -// /** -// * Get all the feedback signal from the interfaces -// * @return true in case of success and false otherwise. -// */ -// bool getFeedbacks(); - -// /** -// * Implements a deadzone for the joypad -// * @return joypad inputs after applying deadzone. -// */ -// double deadzone(const double&); - -// /** -// * Evaluate the desired fingers velocity -// * @param squeezeIndex index used for squeezing -// * @param releaseIndex index used for releasing -// * @return the the desired joints velocity for the fingers in radiant / seconds -// */ -// double evaluateDesiredFingersVelocity(unsigned int squeezeIndex, unsigned int releaseIndex); - -// /** -// * Get the transformation from the transform server -// * @return true in case of success and false otherwise. -// */ -// bool getTransforms(); - -// /** -// * Open the logger -// * @return true if it could open the logger -// */ -// bool openLogger(); - public: OpenXRModule(); ~OpenXRModule(); diff --git a/modules/OpenXR_module/src/OpenXRModule.cpp b/modules/OpenXR_module/src/OpenXRModule.cpp index e6df1f86d..3ec2512b8 100644 --- a/modules/OpenXR_module/src/OpenXRModule.cpp +++ b/modules/OpenXR_module/src/OpenXRModule.cpp @@ -6,6 +6,11 @@ * @date 2021 */ +#include +#include +#include +#include + // YARP #include #include @@ -13,6 +18,7 @@ #include #include #include +#include #include #include @@ -21,8 +27,6 @@ #include #include -#include - Eigen::Ref> getRotation(yarp::sig::Matrix& m) { return Eigen::Map>(m.data()).topLeftCorner<3, 3>(); @@ -60,6 +64,21 @@ struct OpenXRModule::Impl 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 */ + + 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; @@ -78,6 +97,20 @@ struct OpenXRModule::Impl 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. */ @@ -87,8 +120,7 @@ struct OpenXRModule::Impl 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 (NOT - SUPPORTED BY YARP)*/ + 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 */ @@ -104,6 +136,18 @@ struct OpenXRModule::Impl double playerOrientation{0}; /**< Player orientation (read by the Virtualizer) only yaw. */ + bool leftAndRightSwapped{false}; + std::vector buttonsState; + + bool isButtonStateEqualToMask(const std::vector& mask) + { + auto isEqual = [](const auto& v1, const auto& v2) { + return (v1.size() == v2.size() && std::equal(v1.begin(), v1.end(), v2.begin())); + }; + + return isEqual(mask, this->buttonsState); + } + void initializeNeckJointsSmoother(const unsigned actuatedDOFs, const double dT, const double smoothingTime, @@ -176,40 +220,49 @@ struct OpenXRModule::Impl Eigen::Map(this->openXRHeadsetPoseInertial.data()) = getPosition(this->openXRRoot_T_headOpenXR); - // if (!m_useXsens && !m_useIFeel) - // { - - // if (!m_frameTransformInterface->frameExists(m_leftHandFrameName)) - // { - - // yError() << "[OpenXRModule::getTransforms] No " << m_leftHandFrameName << " - // frame."; return false; - // } - - // if (!m_frameTransformInterface->frameExists(m_rightHandFrameName)) - // { - // yError() << "[OpenXRModule::getTransforms] No " << m_rightHandFrameName - // << " frame."; - // return false; - // } - - // if (!m_frameTransformInterface->getTransform( - // m_leftHandFrameName, m_rootFrameName, m_oculusRoot_T_lOculus)) - // { - // yError() << "[OpenXRModule::getTransforms] Unable to evaluate the " - // << m_leftHandFrameName << " to " << m_rootFrameName << "transformation"; - // return false; - // } - - // if (!m_frameTransformInterface->getTransform( - // m_rightHandFrameName, m_rootFrameName, m_oculusRoot_T_rOculus)) - // { - // yError() << "[OpenXRModule::getTransforms] Unable to evaluate the " - // << m_rightHandFrameName << " to " << m_rootFrameName << - // "transformation"; - // return false; - // } - // } + 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; } @@ -325,33 +378,261 @@ struct OpenXRModule::Impl } - // TODO set index of joypad + // 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 - // bool useLeftStick = config.check("use_left", yarp::os::Value("false")).asBool(); - // m_xJoypadIndex = useLeftStick ? 4 : 6; - // m_yJoypadIndex = useLeftStick ? 5 : 7; + // 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); - // m_squeezeLeftIndex = 0; - // m_squeezeRightIndex = 1; + 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); - // m_releaseLeftIndex = 2; - // m_releaseRightIndex = 3; + 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); - // m_startWalkingIndex = 4; // X button - // m_stopWalkingIndex = 5; // X button - // m_prepareWalkingIndex = 0; // A button + 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; + } + } + + 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) ? rightXIndex : leftXIndex; + this->joypadParameters.yJoypadIndex + = (!useLeftStick != !this->leftAndRightSwapped) ? rightYIndex : leftYIndex; + + // 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)) + 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."; @@ -398,6 +679,67 @@ struct OpenXRModule::Impl 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() @@ -461,72 +803,67 @@ bool OpenXRModule::configure(yarp::os::ResourceFinder& rf) return false; } - // if (!m_useSenseGlove) - // { - // // configure fingers retargeting - // m_leftHandFingers = std::make_unique(); - // yarp::os::Bottle& leftFingersOptions = rf.findGroup("LEFT_FINGERS_RETARGETING"); - // leftFingersOptions.append(generalOptions); - // if (!m_leftHandFingers->configure(leftFingersOptions, getName())) - // { - // yError() - // << "[OpenXRModule::configure] Unable to initialize the left fingers retargeting."; - // return false; - // } - - // m_rightHandFingers = std::make_unique(); - // yarp::os::Bottle& rightFingersOptions = rf.findGroup("RIGHT_FINGERS_RETARGETING"); - // rightFingersOptions.append(generalOptions); - // if (!m_rightHandFingers->configure(rightFingersOptions, getName())) - // { - // yError() - // << "[OpenXRModule::configure] Unable to initialize the right fingers 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_leftHand = std::make_unique(); - // yarp::os::Bottle& leftHandOptions = rf.findGroup("LEFT_HAND_RETARGETING"); - // leftHandOptions.append(generalOptions); - // if (!m_leftHand->configure(leftHandOptions)) - // { - // yError() << "[OpenXRModule::configure] Unable to initialize the left fingers retargeting."; - // return false; - // } + 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_rightHand = std::make_unique(); - // yarp::os::Bottle& rightHandOptions = rf.findGroup("RIGHT_HAND_RETARGETING"); - // rightHandOptions.append(generalOptions); - // if (!m_rightHand->configure(rightHandOptions)) - // { - // yError() << "[OpenXRModule::configure] Unable to initialize the right 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_leftHandPosePort.open("/" + getName() + portName)) - // { - // yError() << "[OpenXRModule::configure] Unable to open the port " << portName; - // 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_rightHandPosePort.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")) // { @@ -550,16 +887,16 @@ bool OpenXRModule::configure(yarp::os::ResourceFinder& rf) // return false; // } - // if (!YarpHelper::getStringFromSearchable(rf, "rpcWalkingPort_name", portName)) - // { - // yError() << "[OpenXRModule::configure] Unable to get a string from a searchable"; - // return false; - // } - // if (!m_rpcWalkingClient.open("/" + getName() + portName)) - // { - // yError() << "[OpenXRModule::configure] " << portName << " port already open."; - // 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)) // { @@ -641,20 +978,6 @@ bool OpenXRModule::close() return true; } -// double OpenXRModule::evaluateDesiredFingersVelocity(unsigned int squeezeIndex, -// unsigned int releaseIndex) -// { -// double releaseFingersVelocity, squeezeFingersVelocity; -// m_joypadControllerInterface->getAxis(squeezeIndex, squeezeFingersVelocity); -// m_joypadControllerInterface->getAxis(releaseIndex, releaseFingersVelocity); - -// if (squeezeFingersVelocity > releaseFingersVelocity) -// return squeezeFingersVelocity; -// else if (squeezeFingersVelocity < releaseFingersVelocity) -// return -releaseFingersVelocity; -// else -// return 0; -// } bool OpenXRModule::updateModule() { @@ -664,6 +987,8 @@ bool OpenXRModule::updateModule() return false; } + m_pImpl->buttonsState = m_pImpl->getDeviceButtonsState(); + if (m_pImpl->state == Impl::OpenXRFSM::Running) { @@ -677,8 +1002,6 @@ bool OpenXRModule::updateModule() m_pImpl->head->setPlayerOrientation(m_pImpl->playerOrientation); m_pImpl->head->setDesiredHeadOrientationFromOpenXr(m_pImpl->openXRRoot_T_headOpenXR); - // m_head->setDesiredHeadOrientation(desiredHeadOrientationVector(0), - // desiredHeadOrientationVector(1), desiredHeadOrientationVector(2)); if (m_pImpl->moveRobot) { if (!m_pImpl->head->move()) @@ -687,144 +1010,119 @@ bool OpenXRModule::updateModule() return false; } } - // if (!m_useXsens && !m_useIFeel) - // { - // // update left hand transformation values - // yarp::sig::Vector& leftHandPose = m_leftHandPosePort.prepare(); - // m_leftHand->setPlayerOrientation(m_playerOrientation); - // m_leftHand->setHandTransform(m_oculusRoot_T_lOculus); - - // // update right hand transformation values - // yarp::sig::Vector& rightHandPose = m_rightHandPosePort.prepare(); - // m_rightHand->setPlayerOrientation(m_playerOrientation); - // m_rightHand->setHandTransform(m_oculusRoot_T_rOculus); - - // if (m_useVirtualizer) - // { - // if (std::abs(m_playerOrientation - m_playerOrientationOld) - // > m_playerOrientationThreshold) - // { - // iDynTree::Position teleopPosition = {m_oculusHeadsetPoseInertial[0], - // m_oculusHeadsetPoseInertial[1], - // m_oculusHeadsetPoseInertial[2]}; - - // m_leftHand->setPlayerPosition(teleopPosition); - // m_rightHand->setPlayerPosition(teleopPosition); - // m_playerOrientationOld = m_playerOrientation; - // } - // } - - // // evaluate the robot hands' pose - // m_leftHand->evaluateDesiredHandPose(leftHandPose); - // m_rightHand->evaluateDesiredHandPose(rightHandPose); - - // // move the robot - // if (m_moveRobot) - // { - // m_leftHandPosePort.write(); - // m_rightHandPosePort.write(); - // } - // } - - // use joypad - // std::vector locCmd; - // if (!m_useVirtualizer) - // { - // yarp::os::Bottle cmd, outcome; - // double x, y; - // m_joypadControllerInterface->getAxis(m_xJoypadIndex, x); - // m_joypadControllerInterface->getAxis(m_yJoypadIndex, y); - - // x = -m_scaleX * deadzone(x); - // y = m_scaleY * deadzone(y); - // std::swap(x, y); - - // cmd.addString("setGoal"); - // cmd.addDouble(x); - // cmd.addDouble(y); - // if (m_moveRobot) - // { - // m_rpcWalkingClient.write(cmd, outcome); - // } - // locCmd.push_back(x); - // locCmd.push_back(y); - // } - - // if (!m_useSenseGlove) - // { - // // left fingers - // double leftFingersVelocity - // = evaluateDesiredFingersVelocity(m_squeezeLeftIndex, m_releaseLeftIndex); - // if (!m_leftHandFingers->setFingersVelocity(leftFingersVelocity)) - // { - // yError() << "[OpenXRModule::updateModule] Unable to set the left finger - // velocity."; return false; - // } - // if (m_moveRobot) - // { - // if (!m_leftHandFingers->move()) - // { - // yError() << "[OpenXRModule::updateModule] Unable to move the left finger"; - // return false; - // } - // } - - // // right fingers - // double rightFingersVelocity - // = evaluateDesiredFingersVelocity(m_squeezeRightIndex, m_releaseRightIndex); - // if (!m_rightHandFingers->setFingersVelocity(rightFingersVelocity)) - // { - // yError() << "[OpenXRModule::updateModule] Unable to set the right finger - // velocity."; return false; - // } - // if (m_moveRobot) - // { - // if (!m_rightHandFingers->move()) - // { - // yError() << "[OpenXRModule::updateModule] Unable to move the right finger"; - // return false; - // } - // } - // } - // // check if it is time to prepare or start walking - // float buttonMapping; - - // // prepare robot (A button) - // m_joypadControllerInterface->getButton(m_stopWalkingIndex, buttonMapping); - // yarp::os::Bottle cmd, outcome; - - // if (buttonMapping > 0) - // { - // // TODO add a visual feedback for the user - // if (m_moveRobot) - // { - // cmd.addString("stopWalking"); - // m_rpcWalkingClient.write(cmd, outcome); - // } - // yInfo() << "[OpenXRModule::updateModule] stop"; - // 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(); + } + + // send commands to the walking + yarp::os::Bottle cmd, outcome; + double x, y; + 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); + + cmd.addString("setGoal"); + cmd.addDouble(x); + cmd.addDouble(y); + if (m_pImpl->moveRobot) + { + m_pImpl->rpcWalkingClient.write(cmd, outcome); + } + + + // const std::vector& fingersSqueezeButtonsMask, + // const std::vector& fingersReleaseButtonsMask, + // int fingersVelocityIndex) + + // 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 - // float buttonMapping; - - // // prepare robot (A button) - // m_joypadControllerInterface->getButton(m_prepareWalkingIndex, buttonMapping); - // yarp::os::Bottle cmd, outcome; - - // if (buttonMapping > 0) - // { - // // TODO add a visual feedback for the user - // if (m_moveRobot) - // { - // cmd.addString("prepareRobot"); - // m_rpcWalkingClient.write(cmd, outcome); - // } - // m_state = OculusFSM::InPreparation; - // yInfo() << "[OpenXRModule::updateModule] prepare the robot"; - // } + 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) @@ -837,95 +1135,58 @@ bool OpenXRModule::updateModule() } } - // TODO reenable the initial pose resetting - // float buttonMapping; - // start walking (X button) - - - // m_joypadControllerInterface->getButton(m_startWalkingIndex, buttonMapping); - // yarp::os::Bottle cmd, outcome; - - // if (buttonMapping > 0) - // { - // if (m_useOpenXr) - // { - // if (!m_frameTransformInterface->frameExists(m_headFrameName)) - // { - // yError() << "[OpenXRModule::updateModule] The frame named " << m_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_frameTransformInterface->getTransform( - // m_headFrameName, m_rootFrameName, openXrHeadInitialTransform)) - // { - // yError() << "[OpenXRModule::updateModule] Unable to evaluate the " - // << m_headFrameName << " to " << m_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 yaw = 0; - // double dummy = 0; - // tempRot.getRPY(dummy, yaw, dummy); - - // iDynTree::Transform tempTransform; - // tempTransform.setRotation(iDynTree::Rotation::RotY( - // yaw)); // We remove only the initial rotation of the person head around gravity. - // tempTransform.setPosition(iDynTree::make_span(getPosition( - // openXrHeadInitialTransform))); // We remove the initial position between the - // // head and the reference frame. - - // iDynTree::toEigen(m_openXrInitialAlignement) - // = iDynTree::toEigen(tempTransform.inverse().asHomogeneousTransform()); - // } - - // if (m_useVirtualizer) - // { - // // not sure if here causes the problem of hand rotation, check it - // // reset the player orientation of the virtualizer - // cmd.addString("resetPlayerOrientation"); - // m_rpcVirtualizerClient.write(cmd, outcome); - // cmd.clear(); - // } - - // // TODO add a visual feedback for the user - // if (m_moveRobot) - // { - // cmd.addString("startWalking"); - // m_rpcWalkingClient.write(cmd, outcome); - // } - - // // if(outcome.get(0).asBool()) - // m_state = OculusFSM::Running; - // yInfo() << "[OpenXRModule::updateModule] start the robot"; - // yInfo() << "[OpenXRModule::updateModule] Running ..."; - // } - } + 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 yaw = 0; + double dummy = 0; + tempRot.getRPY(dummy, yaw, dummy); + iDynTree::Transform tempTransform; + tempTransform.setRotation(iDynTree::Rotation::RotY( + yaw)); // We remove only the initial rotation of the person head around gravity. + tempTransform.setPosition(iDynTree::make_span( + getPosition(openXrHeadInitialTransform))); // We remove the initial position between + // the head and the reference frame. + + iDynTree::toEigen(m_pImpl->openXRInitialAlignement) + = iDynTree::toEigen(tempTransform.inverse().asHomogeneousTransform()); + + 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; } - -// double OpenXRModule::deadzone(const double& input) -// { -// if (input >= 0) -// { -// if (input > m_deadzone) -// return (input - m_deadzone) / (m_fullscale - m_deadzone); -// else -// return 0.0; -// } else -// { -// if (input < -m_deadzone) -// return (input + m_deadzone) / (m_fullscale - m_deadzone); -// else -// return 0.0; -// } -// } From 5d61cda43c5444cec384b7d8c36d3fe87a41ef7a Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 30 Sep 2021 10:26:42 +0200 Subject: [PATCH 05/18] Add openXR module configuration files for iCubGenova09 --- .../iCubGenova09/headRetargetingParams.ini | 8 +++ .../leftFingersRetargetingParams.ini | 6 ++ .../leftHandRetargetingParams.ini | 17 +++++ .../robots/iCubGenova09/openXRConfig.ini | 67 +++++++++++++++++++ .../rightFingersRetargetingParams.ini | 6 ++ .../rightHandRetargetingParams.ini | 17 +++++ 6 files changed, 121 insertions(+) create mode 100644 modules/OpenXR_module/config/robots/iCubGenova09/headRetargetingParams.ini create mode 100644 modules/OpenXR_module/config/robots/iCubGenova09/leftFingersRetargetingParams.ini create mode 100644 modules/OpenXR_module/config/robots/iCubGenova09/leftHandRetargetingParams.ini create mode 100644 modules/OpenXR_module/config/robots/iCubGenova09/openXRConfig.ini create mode 100644 modules/OpenXR_module/config/robots/iCubGenova09/rightFingersRetargetingParams.ini create mode 100644 modules/OpenXR_module/config/robots/iCubGenova09/rightHandRetargetingParams.ini 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 000000000..a08d72ebb --- /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 000000000..9307768d5 --- /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 000000000..1bb1db46d --- /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 0.0 -1.0), (-1.0 0.0 0.0), (0.0 1.0 0.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 ((1.0 0.0 0.0), (0.0 1.0 0.0), (0.0 0.0 1.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 000000000..a19e9f971 --- /dev/null +++ b/modules/OpenXR_module/config/robots/iCubGenova09/openXRConfig.ini @@ -0,0 +1,67 @@ +name openXRRetargeting + +# 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 (0, 0, 0, 0, 1, 0, 0, 0) +start_walking_buttons_map (1, 0, 0, 0, 0, 0, 0, 0) +left_fingers_squeeze_buttons_map (0, 0, 0, 0, 0, 0, 0, 0) +left_fingers_release_buttons_map (0, 1, 0, 0, 0, 0, 0, 0) +right_fingers_squeeze_buttons_map (0, 0, 0, 0, 0, 0, 0, 0) +right_fingers_release_buttons_map (0, 0, 0, 0, 0, 1, 0, 0) \ No newline at end of file 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 000000000..d4af06648 --- /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 000000000..1bb1db46d --- /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 0.0 -1.0), (-1.0 0.0 0.0), (0.0 1.0 0.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 ((1.0 0.0 0.0), (0.0 1.0 0.0), (0.0 0.0 1.0)) From 176fedb8881bad39cb9affcbd6f71ccc859999c2 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 30 Sep 2021 10:45:15 +0200 Subject: [PATCH 06/18] Add openXR module configuration files for iCubGazeboV3 --- .../iCubGazeboV3/headRetargetingParams.ini | 8 +++ .../leftFingersRetargetingParams.ini | 6 ++ .../leftHandRetargetingParams.ini | 17 +++++ .../robots/iCubGazeboV3/openXRConfig.ini | 67 +++++++++++++++++++ .../rightFingersRetargetingParams.ini | 6 ++ .../rightHandRetargetingParams.ini | 17 +++++ 6 files changed, 121 insertions(+) create mode 100644 modules/OpenXR_module/config/robots/iCubGazeboV3/headRetargetingParams.ini create mode 100644 modules/OpenXR_module/config/robots/iCubGazeboV3/leftFingersRetargetingParams.ini create mode 100644 modules/OpenXR_module/config/robots/iCubGazeboV3/leftHandRetargetingParams.ini create mode 100644 modules/OpenXR_module/config/robots/iCubGazeboV3/openXRConfig.ini create mode 100644 modules/OpenXR_module/config/robots/iCubGazeboV3/rightFingersRetargetingParams.ini create mode 100644 modules/OpenXR_module/config/robots/iCubGazeboV3/rightHandRetargetingParams.ini 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 000000000..a08d72ebb --- /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 000000000..901de4c19 --- /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 000000000..1bb1db46d --- /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 0.0 -1.0), (-1.0 0.0 0.0), (0.0 1.0 0.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 ((1.0 0.0 0.0), (0.0 1.0 0.0), (0.0 0.0 1.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 000000000..ba16c8edf --- /dev/null +++ b/modules/OpenXR_module/config/robots/iCubGazeboV3/openXRConfig.ini @@ -0,0 +1,67 @@ +name openXRRetargeting + +# 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 (0, 0, 0, 0, 1, 0, 0, 0) +start_walking_buttons_map (1, 0, 0, 0, 0, 0, 0, 0) +left_fingers_squeeze_buttons_map (0, 0, 0, 0, 0, 0, 0, 0) +left_fingers_release_buttons_map (0, 1, 0, 0, 0, 0, 0, 0) +right_fingers_squeeze_buttons_map (0, 0, 0, 0, 0, 0, 0, 0) +right_fingers_release_buttons_map (0, 0, 0, 0, 0, 1, 0, 0) 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 000000000..901de4c19 --- /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 000000000..1bb1db46d --- /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 0.0 -1.0), (-1.0 0.0 0.0), (0.0 1.0 0.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 ((1.0 0.0 0.0), (0.0 1.0 0.0), (0.0 0.0 1.0)) From a07f575d381b8477dba27043f868b39158d6886c Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 30 Sep 2021 22:54:25 +0200 Subject: [PATCH 07/18] Change the logic behind the buttons map in the OpenXRModule --- .../robots/iCubGazeboV3/openXRConfig.ini | 16 ++-- .../robots/iCubGenova09/openXRConfig.ini | 17 ++-- modules/OpenXR_module/src/OpenXRModule.cpp | 78 ++++++++++++------- 3 files changed, 73 insertions(+), 38 deletions(-) diff --git a/modules/OpenXR_module/config/robots/iCubGazeboV3/openXRConfig.ini b/modules/OpenXR_module/config/robots/iCubGazeboV3/openXRConfig.ini index ba16c8edf..86d9948ed 100644 --- a/modules/OpenXR_module/config/robots/iCubGazeboV3/openXRConfig.ini +++ b/modules/OpenXR_module/config/robots/iCubGazeboV3/openXRConfig.ini @@ -59,9 +59,13 @@ scale_Y 5.0 # {"/user/hand/right/input/squeeze/click", "vive_right_squeeze_click"}, # {"/user/hand/right/input/trackpad/click", "vive_right_trackpad_click"} -prepare_walking_buttons_map (0, 0, 0, 0, 1, 0, 0, 0) -start_walking_buttons_map (1, 0, 0, 0, 0, 0, 0, 0) -left_fingers_squeeze_buttons_map (0, 0, 0, 0, 0, 0, 0, 0) -left_fingers_release_buttons_map (0, 1, 0, 0, 0, 0, 0, 0) -right_fingers_squeeze_buttons_map (0, 0, 0, 0, 0, 0, 0, 0) -right_fingers_release_buttons_map (0, 0, 0, 0, 0, 1, 0, 0) +prepare_walking_buttons_map ( 0, 0, 0, 0, 1, 0, 0, 0) # right menu ONLY +start_walking_buttons_map ( 1, 0, 0, 0, 0, 0, 0, 0) # 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/openXRConfig.ini b/modules/OpenXR_module/config/robots/iCubGenova09/openXRConfig.ini index a19e9f971..3a0788a33 100644 --- a/modules/OpenXR_module/config/robots/iCubGenova09/openXRConfig.ini +++ b/modules/OpenXR_module/config/robots/iCubGenova09/openXRConfig.ini @@ -59,9 +59,14 @@ scale_Y 5.0 # {"/user/hand/right/input/squeeze/click", "vive_right_squeeze_click"}, # {"/user/hand/right/input/trackpad/click", "vive_right_trackpad_click"} -prepare_walking_buttons_map (0, 0, 0, 0, 1, 0, 0, 0) -start_walking_buttons_map (1, 0, 0, 0, 0, 0, 0, 0) -left_fingers_squeeze_buttons_map (0, 0, 0, 0, 0, 0, 0, 0) -left_fingers_release_buttons_map (0, 1, 0, 0, 0, 0, 0, 0) -right_fingers_squeeze_buttons_map (0, 0, 0, 0, 0, 0, 0, 0) -right_fingers_release_buttons_map (0, 0, 0, 0, 0, 1, 0, 0) \ No newline at end of file + +prepare_walking_buttons_map ( 0, 0, 0, 0, 1, 0, 0, 0) # right menu ONLY +start_walking_buttons_map ( 1, 0, 0, 0, 0, 0, 0, 0) # 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/src/OpenXRModule.cpp b/modules/OpenXR_module/src/OpenXRModule.cpp index 3ec2512b8..a640a13ce 100644 --- a/modules/OpenXR_module/src/OpenXRModule.cpp +++ b/modules/OpenXR_module/src/OpenXRModule.cpp @@ -75,6 +75,7 @@ struct OpenXRModule::Impl 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 @@ -139,13 +140,17 @@ struct OpenXRModule::Impl bool leftAndRightSwapped{false}; std::vector buttonsState; - bool isButtonStateEqualToMask(const std::vector& mask) + bool isButtonStateEqualToMask(const std::vector& mask) const { - auto isEqual = [](const auto& v1, const auto& v2) { - return (v1.size() == v2.size() && std::equal(v1.begin(), v1.end(), v2.begin())); - }; - - return isEqual(mask, this->buttonsState); + 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, @@ -461,6 +466,29 @@ struct OpenXRModule::Impl } } + 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())); }; @@ -499,7 +527,6 @@ struct OpenXRModule::Impl 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; @@ -518,6 +545,9 @@ struct OpenXRModule::Impl = (!useLeftStick != !this->leftAndRightSwapped) ? rightXIndex : leftXIndex; this->joypadParameters.yJoypadIndex = (!useLeftStick != !this->leftAndRightSwapped) ? rightYIndex : leftYIndex; + this->joypadParameters.joypadButtonsMap = (!useLeftStick != !this->leftAndRightSwapped) + ? rightWalkingButtonsMap + : leftWalkingButtonsMap; // if swapped we have to swap all the maps if (this->leftAndRightSwapped) @@ -1031,29 +1061,25 @@ bool OpenXRModule::updateModule() m_pImpl->rightHandPosePort.write(); } - // send commands to the walking - yarp::os::Bottle cmd, outcome; - double x, y; - 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); - - cmd.addString("setGoal"); - cmd.addDouble(x); - cmd.addDouble(y); - if (m_pImpl->moveRobot) + if (m_pImpl->moveRobot + && m_pImpl->isButtonStateEqualToMask(m_pImpl->joypadParameters.joypadButtonsMap)) { + // send commands to the walking + yarp::os::Bottle cmd, outcome; + double x, y; + 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); + + cmd.addString("setGoal"); + cmd.addDouble(x); + cmd.addDouble(y); m_pImpl->rpcWalkingClient.write(cmd, outcome); } - - // const std::vector& fingersSqueezeButtonsMask, - // const std::vector& fingersReleaseButtonsMask, - // int fingersVelocityIndex) - // left fingers const double leftFingersVelocity = m_pImpl->evaluateDesiredFingersVelocity( m_pImpl->joypadParameters.leftFingersSqueezeButtonsMap, From 1883a74cdcbacc6bb353e0084458fcf4a6d045af Mon Sep 17 00:00:00 2001 From: Ines Date: Fri, 1 Oct 2021 11:42:02 +0200 Subject: [PATCH 08/18] Find correct transforms from openxr to robot --- .../config/robots/iCubGazeboV3/leftHandRetargetingParams.ini | 4 ++-- .../config/robots/iCubGazeboV3/rightHandRetargetingParams.ini | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/modules/OpenXR_module/config/robots/iCubGazeboV3/leftHandRetargetingParams.ini b/modules/OpenXR_module/config/robots/iCubGazeboV3/leftHandRetargetingParams.ini index 1bb1db46d..da696d849 100644 --- a/modules/OpenXR_module/config/robots/iCubGazeboV3/leftHandRetargetingParams.ini +++ b/modules/OpenXR_module/config/robots/iCubGazeboV3/leftHandRetargetingParams.ini @@ -1,6 +1,6 @@ # additional rotations # the following rotation map the hand oculus frame and the hand robot frame -handOculusFrame_R_handRobotFrame ((0.0 0.0 -1.0), (-1.0 0.0 0.0), (0.0 1.0 0.0)) +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 @@ -14,4 +14,4 @@ handOculusFrame_R_handRobotFrame ((0.0 0.0 -1.0), (-1.0 0.0 0.0), (0.0 1.0 0.0)) # 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 ((1.0 0.0 0.0), (0.0 1.0 0.0), (0.0 0.0 1.0)) +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/rightHandRetargetingParams.ini b/modules/OpenXR_module/config/robots/iCubGazeboV3/rightHandRetargetingParams.ini index 1bb1db46d..da696d849 100644 --- a/modules/OpenXR_module/config/robots/iCubGazeboV3/rightHandRetargetingParams.ini +++ b/modules/OpenXR_module/config/robots/iCubGazeboV3/rightHandRetargetingParams.ini @@ -1,6 +1,6 @@ # additional rotations # the following rotation map the hand oculus frame and the hand robot frame -handOculusFrame_R_handRobotFrame ((0.0 0.0 -1.0), (-1.0 0.0 0.0), (0.0 1.0 0.0)) +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 @@ -14,4 +14,4 @@ handOculusFrame_R_handRobotFrame ((0.0 0.0 -1.0), (-1.0 0.0 0.0), (0.0 1.0 0.0)) # 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 ((1.0 0.0 0.0), (0.0 1.0 0.0), (0.0 0.0 1.0)) +teleoperationRobotFrame_R_teleoperationFrame ((0.0 0.0 -1.0), (-1.0 0.0 0.0), (0.0 1.0 0.0)) From 7204e50dd3185b03dccc6f2ef41c572daeabfbeb Mon Sep 17 00:00:00 2001 From: Ines Date: Fri, 1 Oct 2021 11:42:32 +0200 Subject: [PATCH 09/18] Change masks to use vive joypad --- .../OpenXR_module/config/robots/iCubGazeboV3/openXRConfig.ini | 4 ++-- .../OpenXR_module/config/robots/iCubGenova09/openXRConfig.ini | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/modules/OpenXR_module/config/robots/iCubGazeboV3/openXRConfig.ini b/modules/OpenXR_module/config/robots/iCubGazeboV3/openXRConfig.ini index 86d9948ed..e39f5a04a 100644 --- a/modules/OpenXR_module/config/robots/iCubGazeboV3/openXRConfig.ini +++ b/modules/OpenXR_module/config/robots/iCubGazeboV3/openXRConfig.ini @@ -59,8 +59,8 @@ scale_Y 5.0 # {"/user/hand/right/input/squeeze/click", "vive_right_squeeze_click"}, # {"/user/hand/right/input/trackpad/click", "vive_right_trackpad_click"} -prepare_walking_buttons_map ( 0, 0, 0, 0, 1, 0, 0, 0) # right menu ONLY -start_walking_buttons_map ( 1, 0, 0, 0, 0, 0, 0, 0) # left menu ONLY +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 diff --git a/modules/OpenXR_module/config/robots/iCubGenova09/openXRConfig.ini b/modules/OpenXR_module/config/robots/iCubGenova09/openXRConfig.ini index 3a0788a33..92cfcfd26 100644 --- a/modules/OpenXR_module/config/robots/iCubGenova09/openXRConfig.ini +++ b/modules/OpenXR_module/config/robots/iCubGenova09/openXRConfig.ini @@ -60,8 +60,8 @@ scale_Y 5.0 # {"/user/hand/right/input/trackpad/click", "vive_right_trackpad_click"} -prepare_walking_buttons_map ( 0, 0, 0, 0, 1, 0, 0, 0) # right menu ONLY -start_walking_buttons_map ( 1, 0, 0, 0, 0, 0, 0, 0) # left menu ONLY +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 From 911dd65e4a94002ce6128592d29f45d2127c6a79 Mon Sep 17 00:00:00 2001 From: Ines Date: Fri, 1 Oct 2021 11:43:08 +0200 Subject: [PATCH 10/18] Fix right and left joypad usage when joypads are swapped --- modules/OpenXR_module/src/OpenXRModule.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/modules/OpenXR_module/src/OpenXRModule.cpp b/modules/OpenXR_module/src/OpenXRModule.cpp index a640a13ce..698563f3f 100644 --- a/modules/OpenXR_module/src/OpenXRModule.cpp +++ b/modules/OpenXR_module/src/OpenXRModule.cpp @@ -542,12 +542,12 @@ struct OpenXRModule::Impl // this is the xor operator // in c++ you can use !A != !B this->joypadParameters.xJoypadIndex - = (!useLeftStick != !this->leftAndRightSwapped) ? rightXIndex : leftXIndex; + = (!useLeftStick != !this->leftAndRightSwapped) ? leftXIndex : rightXIndex; this->joypadParameters.yJoypadIndex - = (!useLeftStick != !this->leftAndRightSwapped) ? rightYIndex : leftYIndex; + = (!useLeftStick != !this->leftAndRightSwapped) ? leftYIndex : rightYIndex; this->joypadParameters.joypadButtonsMap = (!useLeftStick != !this->leftAndRightSwapped) - ? rightWalkingButtonsMap - : leftWalkingButtonsMap; + ? leftWalkingButtonsMap + : rightWalkingButtonsMap; // if swapped we have to swap all the maps if (this->leftAndRightSwapped) From 184be06f2b52377308040f2674cf4ab264eadc10 Mon Sep 17 00:00:00 2001 From: Ines Date: Fri, 1 Oct 2021 11:48:37 +0200 Subject: [PATCH 11/18] Change openXRRetargeting application name in oculusRetargeting --- .../OpenXR_module/config/robots/iCubGazeboV3/openXRConfig.ini | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/OpenXR_module/config/robots/iCubGazeboV3/openXRConfig.ini b/modules/OpenXR_module/config/robots/iCubGazeboV3/openXRConfig.ini index e39f5a04a..7588b235d 100644 --- a/modules/OpenXR_module/config/robots/iCubGazeboV3/openXRConfig.ini +++ b/modules/OpenXR_module/config/robots/iCubGazeboV3/openXRConfig.ini @@ -1,4 +1,4 @@ -name openXRRetargeting +name oculusRetargeting # ports leftHandPosePort /leftHandPose:o From c09e72617fb4cbe18cdb5e3c5d61536a78443aa5 Mon Sep 17 00:00:00 2001 From: Ines Date: Fri, 1 Oct 2021 17:47:06 +0200 Subject: [PATCH 12/18] Set correct transforms from openxr to robot --- .../config/robots/iCubGenova09/leftHandRetargetingParams.ini | 4 ++-- .../config/robots/iCubGenova09/rightHandRetargetingParams.ini | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/modules/OpenXR_module/config/robots/iCubGenova09/leftHandRetargetingParams.ini b/modules/OpenXR_module/config/robots/iCubGenova09/leftHandRetargetingParams.ini index 1bb1db46d..da696d849 100644 --- a/modules/OpenXR_module/config/robots/iCubGenova09/leftHandRetargetingParams.ini +++ b/modules/OpenXR_module/config/robots/iCubGenova09/leftHandRetargetingParams.ini @@ -1,6 +1,6 @@ # additional rotations # the following rotation map the hand oculus frame and the hand robot frame -handOculusFrame_R_handRobotFrame ((0.0 0.0 -1.0), (-1.0 0.0 0.0), (0.0 1.0 0.0)) +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 @@ -14,4 +14,4 @@ handOculusFrame_R_handRobotFrame ((0.0 0.0 -1.0), (-1.0 0.0 0.0), (0.0 1.0 0.0)) # 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 ((1.0 0.0 0.0), (0.0 1.0 0.0), (0.0 0.0 1.0)) +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/rightHandRetargetingParams.ini b/modules/OpenXR_module/config/robots/iCubGenova09/rightHandRetargetingParams.ini index 1bb1db46d..da696d849 100644 --- a/modules/OpenXR_module/config/robots/iCubGenova09/rightHandRetargetingParams.ini +++ b/modules/OpenXR_module/config/robots/iCubGenova09/rightHandRetargetingParams.ini @@ -1,6 +1,6 @@ # additional rotations # the following rotation map the hand oculus frame and the hand robot frame -handOculusFrame_R_handRobotFrame ((0.0 0.0 -1.0), (-1.0 0.0 0.0), (0.0 1.0 0.0)) +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 @@ -14,4 +14,4 @@ handOculusFrame_R_handRobotFrame ((0.0 0.0 -1.0), (-1.0 0.0 0.0), (0.0 1.0 0.0)) # 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 ((1.0 0.0 0.0), (0.0 1.0 0.0), (0.0 0.0 1.0)) +teleoperationRobotFrame_R_teleoperationFrame ((0.0 0.0 -1.0), (-1.0 0.0 0.0), (0.0 1.0 0.0)) From 00fd671acdda4b6bab15de847116cac11c64c4b1 Mon Sep 17 00:00:00 2001 From: Ines Date: Fri, 1 Oct 2021 17:47:41 +0200 Subject: [PATCH 13/18] Change openXRRetargeting application name in oculusRetargeting - iCubGenova09 --- .../OpenXR_module/config/robots/iCubGenova09/openXRConfig.ini | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/OpenXR_module/config/robots/iCubGenova09/openXRConfig.ini b/modules/OpenXR_module/config/robots/iCubGenova09/openXRConfig.ini index 92cfcfd26..98d0081d0 100644 --- a/modules/OpenXR_module/config/robots/iCubGenova09/openXRConfig.ini +++ b/modules/OpenXR_module/config/robots/iCubGenova09/openXRConfig.ini @@ -1,4 +1,4 @@ -name openXRRetargeting +name oculusRetargeting # ports leftHandPosePort /leftHandPose:o From 3c8d394a4e0b83520c63ae2e090e9fea8edd457e Mon Sep 17 00:00:00 2001 From: Ines Date: Fri, 1 Oct 2021 17:48:43 +0200 Subject: [PATCH 14/18] Fix setGoal(0,0) when the joypad buttons are not pressed to stop walking --- modules/OpenXR_module/src/OpenXRModule.cpp | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/modules/OpenXR_module/src/OpenXRModule.cpp b/modules/OpenXR_module/src/OpenXRModule.cpp index 698563f3f..e01dfc2f3 100644 --- a/modules/OpenXR_module/src/OpenXRModule.cpp +++ b/modules/OpenXR_module/src/OpenXRModule.cpp @@ -1061,19 +1061,23 @@ bool OpenXRModule::updateModule() m_pImpl->rightHandPosePort.write(); } - if (m_pImpl->moveRobot - && m_pImpl->isButtonStateEqualToMask(m_pImpl->joypadParameters.joypadButtonsMap)) + if (m_pImpl->moveRobot) { // send commands to the walking yarp::os::Bottle cmd, outcome; - double x, y; - 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); + 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); From 68e84c7bd5c13eb696e97e66d6a46b05f04116dd Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 4 Oct 2021 10:15:09 +0200 Subject: [PATCH 15/18] Add InstallIniFiles cmake function --- CMakeLists.txt | 1 + cmake/InstallIniFiles.cmake | 71 +++++++++++++++++++++++++++++++++++++ 2 files changed, 72 insertions(+) create mode 100644 cmake/InstallIniFiles.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index 0dfcd3be8..f45d686b6 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/cmake/InstallIniFiles.cmake b/cmake/InstallIniFiles.cmake new file mode 100644 index 000000000..4983b9d2b --- /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() From 222e7dd130edd262283b4e92c81835c0da21a05e Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 4 Oct 2021 10:15:48 +0200 Subject: [PATCH 16/18] Enable the installation of the OpenXRModule configuration files --- modules/OpenXR_module/CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/modules/OpenXR_module/CMakeLists.txt b/modules/OpenXR_module/CMakeLists.txt index 88f66b33e..67caa960e 100644 --- a/modules/OpenXR_module/CMakeLists.txt +++ b/modules/OpenXR_module/CMakeLists.txt @@ -61,3 +61,6 @@ else(ENABLE_LOGGER) endif() install(TARGETS ${EXE_TARGET_NAME} DESTINATION bin) + +install_ini_files(NAME WalkingTeleoperationOpenXR + PARENT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/config) From 78fc380f66903069ee0e5b52ba9f9fa88df4996d Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 4 Oct 2021 10:33:22 +0200 Subject: [PATCH 17/18] Add the OpenXR application --- app/scripts/OpenXR-KynDynRetargeting.xml | 49 +++++++++++++++++++++ app/scripts/dcmWalkingRetargetingOpenXR.xml | 34 ++++++++++++++ 2 files changed, 83 insertions(+) create mode 100644 app/scripts/OpenXR-KynDynRetargeting.xml create mode 100644 app/scripts/dcmWalkingRetargetingOpenXR.xml diff --git a/app/scripts/OpenXR-KynDynRetargeting.xml b/app/scripts/OpenXR-KynDynRetargeting.xml new file mode 100644 index 000000000..d95d6f3e9 --- /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 000000000..69aa6352d --- /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 + + + From abc10a8510c4ffea486b45828da140cd17f05362 Mon Sep 17 00:00:00 2001 From: isorrentino Date: Tue, 5 Oct 2021 18:35:42 +0000 Subject: [PATCH 18/18] Fix orientation and position offset for initial alignment in OpenXRModule --- modules/OpenXR_module/src/OpenXRModule.cpp | 59 +++++++++++++++++++--- 1 file changed, 51 insertions(+), 8 deletions(-) diff --git a/modules/OpenXR_module/src/OpenXRModule.cpp b/modules/OpenXR_module/src/OpenXRModule.cpp index e01dfc2f3..4bf87bde5 100644 --- a/modules/OpenXR_module/src/OpenXRModule.cpp +++ b/modules/OpenXR_module/src/OpenXRModule.cpp @@ -1191,19 +1191,62 @@ bool OpenXRModule::updateModule() // get only the yaw axis iDynTree::Rotation tempRot; iDynTree::toEigen(tempRot) = getRotation(openXrHeadInitialTransform); - double yaw = 0; - double dummy = 0; - tempRot.getRPY(dummy, yaw, dummy); + + 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( - yaw)); // We remove only the initial rotation of the person head around gravity. - tempTransform.setPosition(iDynTree::make_span( - getPosition(openXrHeadInitialTransform))); // We remove the initial position between - // the head and the reference frame. + 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) {