Merge pull request #35 from robotology/devel

Merge Devel to Master

.github/workflows/ci.yml

@@ -0,0 +1,189 @@
+name: C++ CI Workflow
+
+on:
+  push:
+  pull_request:
+  schedule:
+  # * is a special character in YAML so you have to quote this string
+  # Execute a "nightly" build at 2 AM UTC
+  - cron:  '0 2 * * *'
+
+
+jobs:
+  build:
+    name: '[${{ matrix.os }}@${{ matrix.build_type }}]'
+    runs-on: ${{ matrix.os }}
+    strategy:
+      matrix:
+        build_type: [Release]
+        os: [ubuntu-latest, windows-latest, macOS-latest]
+
+    steps:
+    - uses: actions/checkout@master
+
+    # Print environment variables to simplify development and debugging
+    - name: Environment Variables
+      shell: bash
+      run: env
+
+    # ============
+    # DEPENDENCIES
+    # ============
+
+    # Remove apt repos that are known to break from time to time 
+    # See https://github.com/actions/virtual-environments/issues/323  
+    - name: Remove broken apt repos [Ubuntu]
+      if: matrix.os == 'ubuntu-latest'
+      run: |
+        for apt_file in `grep -lr microsoft /etc/apt/sources.list.d/`; do sudo rm $apt_file; done
+
+    - name: Dependencies [Windows]
+      if: matrix.os == 'windows-latest'
+      run: |
+        git clone https://github.com/robotology-dependencies/robotology-vcpkg-binary-ports C:/robotology-vcpkg-binary-ports
+        vcpkg.exe --overlay-ports=C:/robotology-vcpkg-binary-ports install --triplet x64-windows ace libxml2 eigen3 ipopt-binary catch2
+
+    - name: Dependencies [macOS]
+      if: matrix.os == 'macOS-latest'
+      run: |
+        brew install ace boost eigen swig qt5 orocos-kdl catch2
+
+    - name: Dependencies [Ubuntu]
+      if: matrix.os == 'ubuntu-latest'
+      run: |
+        sudo apt-get update
+        sudo apt-get install git build-essential cmake libace-dev coinor-libipopt-dev  libboost-system-dev libboost-filesystem-dev \
+                             libboost-thread-dev liborocos-kdl-dev libeigen3-dev swig qtbase5-dev qtdeclarative5-dev qtmultimedia5-dev \
+                             libxml2-dev liburdfdom-dev libtinyxml-dev liburdfdom-dev liboctave-dev python-dev valgrind
+
+    - name: Source-based Dependencies [Windows]
+      if: matrix.os == 'windows-latest'
+      shell: bash
+      run: |
+        # YCM
+        cd ${GITHUB_WORKSPACE}
+        git clone https://github.com/robotology/ycm
+        cd ycm
+        mkdir -p build
+        cd build
+        cmake -A x64 -DCMAKE_TOOLCHAIN_FILE=${VCPKG_INSTALLATION_ROOT}/scripts/buildsystems/vcpkg.cmake \
+                     -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install ..
+
+        cmake --build . --config ${{ matrix.build_type }} --target INSTALL
+
+
+        # YARP
+        cd ${GITHUB_WORKSPACE}
+        git clone https://github.com/robotology/yarp
+        cd yarp
+        mkdir -p build
+        cd build
+        cmake -A x64 -DCMAKE_TOOLCHAIN_FILE=${VCPKG_INSTALLATION_ROOT}/scripts/buildsystems/vcpkg.cmake \
+              -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install ..
+        cmake --build . --config ${{ matrix.build_type }} --target INSTALL
+        # Workaround for https://github.com/robotology-dependencies/robotology-vcpkg-binary-ports/issues/3
+        export IPOPT_DIR=${VCPKG_INSTALLATION_ROOT}/installed/x64-windows
+
+        # iDynTree
+        cd ${GITHUB_WORKSPACE}
+        git clone https://github.com/robotology/iDynTree
+        cd iDynTree
+        git checkout devel
+        mkdir -p build
+        cd build
+        cmake -A x64 -DCMAKE_TOOLCHAIN_FILE=${VCPKG_INSTALLATION_ROOT}/scripts/buildsystems/vcpkg.cmake \
+                     -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install \
+                     -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \
+                     -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install ..
+
+        cmake --build . --config ${{ matrix.build_type }} --target install
+
+        # icub-main
+        cd ${GITHUB_WORKSPACE}
+        git clone https://github.com/robotology/icub-main.git --depth 1 --branch devel
+        cd icub-main && mkdir -p build && cd build
+        cmake  -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \
+               -DENABLE_icubmod_cartesiancontrollerserver=ON -DENABLE_icubmod_cartesiancontrollerclient=ON -DENABLE_icubmod_gazecontrollerclient=ON ..
+        cmake --build . --config ${{ matrix.build_type }} --target install
+        
+
+    - name: Source-based Dependencies [Ubuntu/macOS]
+      if: matrix.os == 'ubuntu-latest' || matrix.os == 'macOS-latest'
+      shell: bash
+      run: |
+        # YCM
+        cd ${GITHUB_WORKSPACE}
+        git clone https://github.com/robotology/ycm
+        cd ycm
+        mkdir -p build
+        cd build
+        cmake -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install ..
+        cmake --build . --config ${{ matrix.build_type }} --target install
+
+        # YARP
+        cd ${GITHUB_WORKSPACE}
+        git clone https://github.com/robotology/yarp
+        cd yarp
+        mkdir -p build
+        cd build
+        cmake  -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install ..
+        cmake --build . --config ${{ matrix.build_type }} --target install
+
+        # iDynTree
+        cd ${GITHUB_WORKSPACE}
+        git clone https://github.com/robotology/iDynTree
+        cd iDynTree
+        git checkout devel
+        mkdir -p build
+        cd build
+        cmake -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \
+              -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install ..
+        cmake --build . --config ${{ matrix.build_type }} --target install
+
+        # icub-main
+        cd ${GITHUB_WORKSPACE}
+        git clone https://github.com/robotology/icub-main.git --depth 1 --branch devel
+        cd icub-main && mkdir -p build && cd build
+        cmake  -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \
+               -DENABLE_icubmod_cartesiancontrollerserver=ON -DENABLE_icubmod_cartesiancontrollerclient=ON -DENABLE_icubmod_gazecontrollerclient=ON ..
+        cmake --build . --config ${{ matrix.build_type }} --target install 
+
+
+    # ===================
+    # CMAKE-BASED PROJECT
+    # ===================
+
+    - name: Configure [Windows]
+      # Use bash also on Windows (otherwise cd, mkdir, ... do not work)
+      if: matrix.os == 'windows-latest'
+      shell: bash
+      run: |
+        mkdir -p build
+        cd build
+        cmake -A x64 -DCMAKE_TOOLCHAIN_FILE=${VCPKG_INSTALLATION_ROOT}/scripts/buildsystems/vcpkg.cmake \
+              -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install \
+              -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install ..
+
+    - name: Configure [Ubuntu/macOS]
+      if: matrix.os == 'ubuntu-latest' || matrix.os == 'macOS-latest'
+      shell: bash
+      run: |
+        mkdir -p build
+        cd build
+        cmake -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install \
+              -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install ..
+
+    - name: Build
+      shell: bash
+      run: |
+        cd build
+        # Attempt of fix for using YARP idl generators (that link ACE) in Windows
+        # See https://github.com/robotology/idyntree/issues/569
+        export PATH=$PATH:${GITHUB_WORKSPACE}/install/bin:${VCPKG_ROBOTOLOGY_ROOT}/installed/x64-windows/bin
+        cmake --build . --config ${{ matrix.build_type }}
+
+    - name: Install
+      shell: bash
+      run: |
+        cd build
+        cmake --build . --config ${{ matrix.build_type }} --target install

.gitignore

@@ -42,4 +42,5 @@ build/
 .\#*\#
 
 TAGS
-compile_commands.json
+compile_commands.json
+CMakeLists.txt.user

CMakeLists.txt

@@ -6,12 +6,13 @@ cmake_minimum_required(VERSION 3.5)
 set(CMAKE_CXX_STANDARD 14)
 
 project(walking-teleoperation
-  VERSION 0.2.0)
+  VERSION 1.0.0)
 
 list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
 
 find_package(YARP REQUIRED)
 find_package(YCM REQUIRED)
+include(WalkingTeleoperationFindDependencies)
 
 add_subdirectory(modules)
 add_subdirectory(app)

README.md

@@ -1,11 +1,27 @@
 # walking-teleoperation
+
 This repository contains software related to walking and teleoperation. 
 
+The whole-body teleoperation architecture is shown as follows.
+The human user receives visual feedback from the robot environment by streaming the robot camera images through the _Oculus Headset_.
+The robot hands are controlled via the _Joypads_.
+The linear and the angular velocities of the human are obtained from the _Cyberith Virtualizer VR Treadmill_. 
+The operator can wear a sensorized full body suit to obtain the kinematic information of various human links with respect to the inertial frame of reference. 
+
+
+![teleoperationArchitecture](https://user-images.githubusercontent.com/17707730/75995333-8d15b100-5efc-11ea-8a40-cea64bf36bf8.jpg)
+
+
+Software related to walking and teleoperation. 
+
 The suite includes:
 
 * **Oculus_module**: this is the module that implements retargeting of the upper body end_effectors.
-* **Virtualizer_module**: this module allows using the cyberith virtualizer as a joypad interface for walking commands.
-* **Utils_module**: an module that can be useful to implement some common functionality
+* **Virtualizer_module**: this module allows using the Cyberith virtualizer as a joypad interface for walking commands.
+* **Utils_module**: a module that can be useful to implement some common functionality
+* **Xsens_module**: a module that gets joint values from [human state provider](https://github.com/robotology/human-dynamics-estimation/) and maps them to the [walking controller](https://github.com/robotology/walking-controllers) input
+
+The technical description of the suit and the frame descriptions are documented [here](./docs/FrameDescriptions.md).
 
 # Overview
  - [:orange_book: The general idea](#orange_book-some-theory-behind-the-code)
@@ -14,21 +30,23 @@ The suite includes:
  - [:running: Using the software with iCub](#running-using-the-software-with-iCub)
 
 # :orange_book: The general idea
-This software allows teleoperation of a walking humanoid robot with a walking controller that expects positions on the plane as walking direction commands for the planner.
+This software allows teleoperation of a biped humanoid robot,e.g., iCub, with a walking controller that expects speed and orientation of the robot locomotion on a horizontal plane and commands for upper body control of the humanoid robot.
 It implements the following architecture:
-* Oculus module that captures the end effectors of the hands and head of the human operator and commands the respective movement;
-* Virtualizer module [Optional] that graps the human walking teleoperation commands (orientation and Speed).
-
+* Oculus module: that captures the end effectors of the hands and head of the human operator and commands the respective movement (if only using Oculus VR);
+* Virtualizer module: [Optional]  grasps the human walking teleoperation commands (orientation and Speed).
+* Xsens module: [optional] maps the robot's joints values to the controller
 
 # :page_facing_up: Dependencies
 The description of dependencies are located [here](./docs/Dependencies.md).
 
 This guide is only for teleoperation dependencies on a Windows machine and it includes the guide to install Oculus module and Virtualizer module SDKs.
 
-Besides, you need to have a linux machine for **Walking-controllers** module described [here](https://github.com/robotology/walking-controllers/tree/devel_hand_retargeting).
+If you want to run the teleoperation scenario with Xsens MVN technologies, you need to enable the option [human-dynamics in robotology/superbuild](https://github.com/robotology/robotology-superbuild#human-dynamics) and install all the dependencies described there.
+
+Besides, you need to have a Linux machine for **Walking-controllers** module described [here](https://github.com/robotology/walking-controllers/tree/devel_hand_retargeting).
 
 # :hammer: Build the suite
-## Linux/macOs
+## Linux/macOS
 
 ```sh
 git clone https://github.com/robotology/walking-teleoperation.git
@@ -43,21 +61,48 @@ Follow the same instructions from the Powershell. One can also opt to use the ``
 
 # :running: Using the software with iCub
 Import the `DCM_WALKING_COORDINATOR_+_RETARGETING` to the `yarpmanager` applications.
-The current set-up allows to run the module either on windows, or from a linux machine through `yarprun --server /name_of_server`. The preference is the following.
-* Turn on the robot, through the linux machine.
+The current set-up allows running the module either on windows or from a Linux machine through `yarprun --server /name_of_server`. The preference is the following.
+* Turn on the robot, through the Linux machine.
 * On the windows machine, use the same network.
 * Do a `yarp namespace /the_robot_network_namespace`
 * Do a `yarprun --server /icub-virtualizer`
 * Calibrate the virtualizer and the oculus
 * At this point, the operator should be in the virtualizer wearing the oculus and in the zero configuration, i.e. zero orientation in the virtualizer, facing the same direction as the robot and standing still.
-* On the linux server, and from the `yarpmanager` run the application `DCM_WALKING_COORDINATOR_+_RETARGETING`
+* On the Linux server, and from the `yarpmanager` run one of the applications `DCM_walking_retargeting`,`DCM_walking_retargeting (Virtualizer)` ,`DCM_walking_retargeting_(Xsens)`, or `DCM_walking_retargeting_(Virtualizer_Xsens)` depending on the experiment you want to perform.
 * On the same application window, connect all the ports.
 * On the windows machine, adjust the image size and positioning (field of view) of the Oculus (to zoom out press ctrl+z, to move the right display use right ctrl+direction, to move the left display use left ctrl+direction ).
-* On the linux machine to adjust the image quality, use the `frameGrapperGui` in the `calib_cams` application.
+* On the Linux machine to adjust the image quality, use the `frameGrapperGui` in the `calib_cams` application.
+
 
 ## :warning: Warning
-Currently the supported robots are only:
+Currently, the supported robots are only:
 - ``iCubGenova04``
 - ``iCubGenova02``
 
+## :eyeglasses: Reference paper
+
+You can read more about the work [here.](https://arxiv.org/pdf/1909.10080.pdf)
+If you're going to use this package for your work, please quote it within any resulting publication:
+```
+@inproceedings{Whole-Body-2019,
+author = {Darvish, Kourosh and Tirupachuri, Yeshasvi and Romualdi, Giulio and Rapetti, Lorenzo and Ferigo, Diego and Chavez, Francisco Javier Andrade and Pucci, Daniele},
+ title = {Whole-Body Geometric Retargeting for Humanoid Robots},
+booktitle = {Proceedings of the 2019 IEEE/RAS International Conference on Humanoid Robots (Humanoids)},
+year = {2019},
+month={October},
+address = {Toronto, Canada},
+}
+```
+## :dollar: License
+The _walking-teleoperation_ repository is licensed under either the GNU Lesser General Public License v3.0 :
+
+https://www.gnu.org/licenses/lgpl-3.0.html
+
+or the GNU Lesser General Public License v2.1 :
+
+https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+
+at your option.
+
 
+**To get the updated information about the dependencies branches and how to run, please check the wiki page of this repository.**

app/robots/iCubGazeboV2_5/XsensRetargetingWalking.ini

@@ -0,0 +1,18 @@
+name                    XsensRetargeting
+samplingTime            0.01
+robot                   icub
+# if the following is enabled we will use smoothing with smoothingTime value (default is true)
+useSmoothing 0
+smoothingTime   0.25
+# The max difference (threshold) of a joint value coming from the human (rad)
+jointDifferenceThreshold 0.5
+wholeBodyJointsPort           /HumanStateWrapper/state:i
+controllerJointsPort     /jointPosition:o
+controllerCoMPort     /CoM:o
+
+# ROBOT JOINT LIST (Notice the order of the joint list is not wrong)
+# Indeed they are written according to the joint order of the walking-coordinator
+joints_list (            "neck_pitch", "neck_roll", "neck_yaw",
+                         "torso_pitch", "torso_roll", "torso_yaw",
+                         "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup",
+                         "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow",  "r_wrist_prosup" )

app/robots/iCubGazeboV2_5/XsensRetargetingYoga.ini

@@ -0,0 +1,20 @@
+name                    XsensRetargeting
+samplingTime            0.01
+robot                   icub
+# if the following is enabled we will use smoothing with smoothingTime value (default is true)
+useSmoothing 1
+smoothingTime   0.25
+# The max difference (threshold) of a joint value coming from the human (rad)
+jointDifferenceThreshold 0.5
+wholeBodyJointsPort           /HumanStateWrapper/state:i
+controllerJointsPort     /jointPosition:o
+controllerCoMPort     /CoM:o
+
+# ROBOT JOINT LIST (Notice the order of the joint list is not wrong)
+# Indeed they are written according to the joint order of the yoga-retargeting
+joints_list (
+                         "torso_pitch", "torso_roll", "torso_yaw",
+                         "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow",
+                         "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow",
+                         "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll",
+                         "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")

app/robots/iCubGazeboV2_5/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)

app/robots/iCubGazeboV2_5/leftFingersRetargetingParams.ini

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