Add htc vive application

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)

app/scripts/OpenXR-KynDynRetargeting.xml

@@ -0,0 +1,49 @@
+<!-- Copyright (C) 2021 Fondazione Istituto Italiano di Tecnologia (IIT) -->
+<!-- All Rights Reserved. -->
+<!-- Author: Giulio Romualdi <[email protected]> -->
+
+<application>
+
+  <name>OpenXR-KynDynRetargeting</name>
+  <description>Application to run kinematic and dynamic related modules for retargeting.</description>
+  <version>1.0</version>
+  <authors>
+    <author email="[email protected]">Giulio Romualdi</author>
+  </authors>
+
+  <!-- Modules -->
+  <module>
+    <name>WalkingModule</name>
+    <node>icub-head</node>
+    <parameters>--from dcm_walking_hand_retargeting.ini</parameters>
+  </module>
+
+  <module>
+    <name>OpenXRModule</name>
+    <node>icub-console-gui</node>
+    <dependencies>
+      <port timeout="5.0">/transformServer/transforms:o</port>
+      <port timeout="5.0">/joypadDevice/Oculus/rpc:i</port>
+      <port timeout="5.0">/icub/cam/left</port>
+      <port timeout="5.0">/icub/cam/right</port>
+    </dependencies>
+  </module>
+
+  <!-- Connections -->
+  <connection>
+    <from>/oculusRetargeting/leftHandPose:o</from>
+    <to>/walking-coordinator/leftHandDesiredPose:i</to>
+  </connection>
+
+  <connection>
+    <from>/oculusRetargeting/rightHandPose:o</from>
+    <to>/walking-coordinator/rightHandDesiredPose:i</to>
+  </connection>
+
+  <connection>
+    <from>/oculusRetargeting/walkingRpc</from>
+    <to>/walking-coordinator/rpc</to>
+  </connection>
+
+
+</application>

app/scripts/dcmWalkingRetargetingOpenXR.xml

@@ -0,0 +1,34 @@
+<!-- Copyright (C) 2021 Fondazione Istituto Italiano di Tecnologia (IIT) -->
+<!-- All Rights Reserved. -->
+<!-- Author: Kourosh Darvish <[email protected]> -->
+
+<application>
+
+  <name>OpenXR application</name>
+  <description>Application to run the retargeting with openXR.</description>
+  <version>1.0</version>
+  <authors>
+    <author email="[email protected]">Giulio Romualdi</author>
+  </authors>
+
+<application>
+   <name>transformServer</name>
+ </application>
+
+<application>
+    <name>Xprize-AudioRetargeting</name>
+  </application>
+
+  <application>
+    <name>Xprize-VisualRetargeting</name>
+  </application>
+
+  <application>
+    <name>OpenXR-KynDynRetargeting</name>
+  </application>
+
+  <application>
+    <name>Xprize-PsychophysiologicalRetargeting</name>
+  </application>
+
+</application>

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()

modules/CMakeLists.txt

@@ -3,7 +3,9 @@
 # Authors: Giulio Romualdi <[email protected]>
 
 add_subdirectory(Utils)
+add_subdirectory(Common)
 add_subdirectory(Oculus_module)
+add_subdirectory(OpenXR_module)
 
 if(WALKING_TELEOPERATION_COMPILE_XsensModule)
   add_subdirectory(Xsens_module)

modules/Common/CMakeLists.txt

@@ -0,0 +1,46 @@
+# Copyright (C) 2021 Fondazione Istituto Italiano di Tecnologia (IIT)
+# All Rights Reserved.
+# Authors: Giulio Romualdi <[email protected]>
+
+# 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)

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 <[email protected]>
 
@@ -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()
 

modules/OpenXR_module/CMakeLists.txt

@@ -0,0 +1,66 @@
+# Copyright (C) 2021 Fondazione Istituto Italiano di Tecnologia (IIT)
+# All Rights Reserved.
+# Authors: Giulio Romualdi <[email protected]>
+
+# set target name
+set(EXE_TARGET_NAME OpenXRRetargetingModule)
+
+option(ENABLE_RPATH "Enable RPATH for this library" ON)
+option(ENABLE_LOGGER "Enable logger using matlogger2" OFF)
+
+mark_as_advanced(ENABLE_RPATH)
+include(AddInstallRPATHSupport)
+add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_BINDIR}"
+  LIB_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}"
+  INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}"
+  DEPENDS ENABLE_RPATH
+  USE_LINK_PATH)
+
+# Find required package
+if(ENABLE_LOGGER)
+  add_definitions(-DENABLE_LOGGER)
+  find_package(matlogger2 REQUIRED)
+endif(ENABLE_LOGGER)
+include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR})
+include(FindPackageHandleStandardArgs)
+
+# set cpp files
+set(${EXE_TARGET_NAME}_SRC
+  src/main.cpp
+  src/OpenXRModule.cpp
+  )
+
+# set hpp files
+set(${EXE_TARGET_NAME}_HDR
+  include/OpenXRModule.hpp
+  )
+
+# add include directories to the build.
+include_directories(
+  ${CMAKE_CURRENT_SOURCE_DIR}/include
+)
+
+# add an executable to the project using the specified source files.
+add_executable(${EXE_TARGET_NAME} ${${EXE_TARGET_NAME}_SRC} ${${EXE_TARGET_NAME}_HDR})
+
+if(ENABLE_LOGGER)
+  target_link_libraries(${EXE_TARGET_NAME} PRIVATE
+    ${YARP_LIBRARIES}
+    ${iDynTree_LIBRARIES}
+    ctrlLib
+    CommonLibrary
+    matlogger2::matlogger2)
+else(ENABLE_LOGGER)
+  target_link_libraries(${EXE_TARGET_NAME} PRIVATE
+    ${YARP_LIBRARIES}
+    ${iDynTree_LIBRARIES}
+    ctrlLib
+    UtilityLibrary
+    CommonLibrary
+)
+endif()
+
+install(TARGETS ${EXE_TARGET_NAME} DESTINATION bin)
+
+install_ini_files(NAME WalkingTeleoperationOpenXR
+                  PARENT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/config)

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)

modules/OpenXR_module/config/robots/iCubGazeboV3/leftFingersRetargetingParams.ini

@@ -0,0 +1,6 @@
+remote_control_boards   ()
+joints_list             ()
+
+useVelocity           1
+
+fingersScaling        ()

modules/OpenXR_module/config/robots/iCubGazeboV3/leftHandRetargetingParams.ini

@@ -0,0 +1,17 @@
+# additional rotations
+# the following rotation map the hand oculus frame and the hand robot frame
+handOculusFrame_R_handRobotFrame ((0.0 -1.0 0.0), (1.0 0.0 0.0), (0.0 0.0 1.0))
+
+# the following rotation map the teleoperation frame and the teleoperation robot
+# frame the Teleoperation robot frame chosen is "imu_frame" and according to iCub
+# CAD has:
+# The z-axis is parallel to gravity but pointing upwards.
+# The x-axis points behind the robot.
+# The y-axis points laterally and is chosen according to the right-hand rule.
+# the Teleoperation frame is attached to the virtualizer and the  origin is the
+# same oculus inertial frame. In other words:
+# The z-axis is parallel to gravity but pointing upwards
+# The x-axis points forward
+# The y-axis points laterally and is chosen according to the right-hand rule.
+# So the rotation matrix is simply given by Rotz(pi)
+teleoperationRobotFrame_R_teleoperationFrame ((0.0 0.0 -1.0), (-1.0 0.0 0.0), (0.0 1.0 0.0))