├── .gitignore ├── CMakeLists.txt ├── LICENSE.md ├── README.md ├── app ├── reaching-SIM.xml └── reaching.xml ├── cmake ├── AddInstallRPATHSupport.cmake ├── AddUninstallTarget.cmake └── InstallBasicPackageFiles.cmake ├── doc └── main.dox └── src ├── CMakeLists.txt ├── hand-tracking ├── CMakeLists.txt ├── app │ ├── hand-tracking-BATCH.xml │ ├── hand-tracking-SIM.xml │ └── hand-tracking.xml ├── conf │ └── config.ini ├── include │ ├── BrownianMotionPose.h │ ├── DrawFwdKinPoses.h │ ├── FwdKinModel.h │ ├── GatePose.h │ ├── InitiCubArm.h │ ├── PlayFwdKinModel.h │ ├── PlayGatePose.h │ ├── VisualProprioception.h │ ├── VisualSIS.h │ ├── VisualUpdateParticles.h │ ├── iCubFwdKinModel.h │ └── iCubGatePose.h ├── mesh │ └── icub_right_arm │ │ ├── r_ail0.obj │ │ ├── r_ail1.obj │ │ ├── r_ail2.obj │ │ ├── r_ail3.obj │ │ ├── r_forearm.obj │ │ ├── r_indexbase.obj │ │ ├── r_ml0.obj │ │ ├── r_ml1.obj │ │ ├── r_ml2.obj │ │ ├── r_ml3.obj │ │ ├── r_palm.obj │ │ ├── r_tl0.obj │ │ ├── r_tl1.obj │ │ ├── r_tl2.obj │ │ ├── r_tl3.obj │ │ └── r_tl4.obj ├── shader │ ├── shader_background.frag │ ├── shader_background.vert │ ├── shader_model.frag │ └── shader_model.vert ├── src │ ├── BrownianMotionPose.cpp │ ├── DrawFwdKinPoses.cpp │ ├── FwdKinModel.cpp │ ├── GatePose.cpp │ ├── InitiCubArm.cpp │ ├── PlayFwdKinModel.cpp │ ├── PlayGatePose.cpp │ ├── VisualProprioception.cpp │ ├── VisualSIS.cpp │ ├── VisualUpdateParticles.cpp │ ├── iCubFwdKinModel.cpp │ ├── iCubGatePose.cpp │ └── main.cpp └── thrift │ └── visualsisparticlefilter.thrift ├── reaching ├── CMakeLists.txt └── main.cpp └── visualservoing ├── CMakeLists.txt ├── visualservoingclient-app ├── CMakeLists.txt └── src │ └── main.cpp ├── visualservoingclient ├── CMakeLists.txt ├── conf │ └── visualservoingclient.ini ├── include │ └── VisualServoingClient.h ├── src │ └── VisualServoingClient.cpp └── thrift │ └── visualservoing.thrift ├── visualservoingcommon ├── app │ ├── BackgroundApps-SIM.xml │ ├── visualservoing-SIM.xml │ ├── visualservoing-click.xml │ ├── visualservoing-datadump.xml │ └── visualservoing.xml └── include │ └── VisualServoingCommon.h ├── visualservoingserver-app ├── CMakeLists.txt └── src │ └── main.cpp └── visualservoingserver ├── CMakeLists.txt ├── conf └── visualservoingserver.ini ├── include └── VisualServoingServer.h ├── src └── VisualServoingServer.cpp └── thrift └── visualservoing.thrift /.gitignore: -------------------------------------------------------------------------------- 1 | # Pre & Compiled source # 2 | ######################### 3 | *.com 4 | *.class 5 | *.dll 6 | *.exe 7 | *.o 8 | *.so 9 | *.out 10 | *.app 11 | *.slo 12 | *.lo 13 | *.gch 14 | *.pch 15 | *.dylib 16 | *.mod 17 | *.lai 18 | *.la 19 | *.a 20 | *.lib 21 | 22 | # Packages # 23 | ############ 24 | # it's better to unpack these files and commit the raw source 25 | # git has its own built in compression methods 26 | *.7z 27 | *.dmg 28 | *.gz 29 | *.iso 30 | *.jar 31 | *.rar 32 | *.tar 33 | *.zip 34 | 35 | # OS generated files # 36 | ###################### 37 | .DS_Store 38 | .DS_Store? 39 | ._* 40 | .Spotlight-V100 41 | .Trashes 42 | ehthumbs.db 43 | Thumbs.db 44 | 45 | # Custom Folders # 46 | ################## 47 | .idea/ 48 | build*/ 49 | xcodeenvvar.txt 50 | *.txt.user 51 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # BSD 3-Clause license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | if(WIN32) 11 | cmake_minimum_required(VERSION 3.4) 12 | else() 13 | cmake_minimum_required(VERSION 3.0) 14 | endif() 15 | 16 | project(visual-tracking-control 17 | LANGUAGES CXX 18 | VERSION 0.5.0.0) 19 | 20 | set(CMAKE_CXX_STANDARD 11) 21 | 22 | include(GNUInstallDirs) 23 | 24 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_BINDIR}") 25 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}") 26 | set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}") 27 | 28 | set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) 29 | 30 | list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) 31 | 32 | if(WIN32) 33 | add_definitions(-D_USE_MATH_DEFINES) 34 | if(MSVC) 35 | set(CMAKE_DEBUG_POSTFIX "d") 36 | set(CMAKE_CXX_FLAGS "/MP /EHsc") 37 | set(CMAKE_C_FLAGS "/MP /EHsc") 38 | endif() 39 | endif() 40 | 41 | ### iCub-contrib project 42 | find_package(ICUBcontrib REQUIRED) 43 | list(APPEND CMAKE_MODULE_PATH ${ICUBCONTRIB_MODULE_PATH}) 44 | include(ICUBcontribHelpers) 45 | include(ICUBcontribOptions) 46 | icubcontrib_set_default_prefix() 47 | icubcontrib_add_uninstall_target() 48 | 49 | # Support RPATH? 50 | option(ENABLE_RPATH "Enable RPATH for this library" ON) 51 | mark_as_advanced(ENABLE_RPATH) 52 | include(AddInstallRPATHSupport) 53 | add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_BINDIR}" 54 | LIB_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}" 55 | INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}" 56 | DEPENDS ENABLE_RPATH 57 | USE_LINK_PATH) 58 | 59 | add_subdirectory(src) 60 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | Copyright (c) 2016, Claudio Fantacci 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | * Redistributions in binary form must reproduce the above copyright notice this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 7 | * Neither the name of the organization nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 8 | 9 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL CLAUDIO FANTACCI BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 10 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ⚙️ visual-tracking-control 2 | 3 | The **visual-tracking-control** project is a _suite_ of cross-platform applications for visual tracking and visual servoing for the humanoid robot platform iCub. 4 | 5 | The suite includes: 6 | - **hand-tracking**: a visual end-effector tracker using a 3D model-aided particle filter [[1]](https://arxiv.org/abs/1703.04771); 7 | - **visual-servoing**: a visual servoing `YARP` plugin to control the pose (position and orientation) of the end-effector using image feedback [[2]](https://arxiv.org/abs/1710.04465). 8 | - The `visual-servoing` consist of two plugins, a client and server version. On one hand, the server must be always running via `yarpdev` and is responsible to command the robot. On the other hand, the client is dynamically loaded by any application using visual servoing interfaces and has the sole purpose of communicating commands to the server. This architecture enable distributed computation over different machine and decouples client and server implementations. 9 | 10 | The output of the hand-tracking application can be visualized by means of the [iCubProprioception](https://github.com/claudiofantacci/iCubProprioception) module. iCubProprioception provides an augmented-reality application which superimposes the 3D model of the end-effector onto the camera images, using the estimated pose provided by hand-tracking. 11 | 12 | [![visual-tracking-control home](https://img.shields.io/badge/Visual%20Tracking%20Control-Home%20%26%20Doc-E0C57F.svg?style=flat-square)](https://robotology.github.io/visual-tracking-control/doxygen/doc/html/index.html) [![ZenHub](https://img.shields.io/badge/Shipping_faster_with-ZenHub-blue.svg?style=flat-square)](https://zenhub.com) 13 | 14 | 15 | # Overview 16 | - [🎛 Dependencies](#-dependencies) 17 | - [🔨 Build the suite](#-build-the-suite) 18 | - [📝 API documentation and example code](#-api-documentaion-and-example-code) 19 | - [📑 Reference](#-reference) 20 | 21 | 22 | # 🎛 Dependencies 23 | visual-tracking-control suite depends on 24 | - [BayesFilters](https://github.com/robotology/bayes-filters-lib) - `version >= 0.6` 25 | - [iCub](https://github.com/robotology/icub-main) 26 | - [iCubContrib](https://github.com/robotology/icub-contrib-common) 27 | - [OpenCV](http://opencv.org) - `version >= 3.3`, built with `CUDA >= 8.0` 28 | - [SuperimposeMesh](https://github.com/robotology/superimpose-mesh-lib) - `version >= 0.9` 29 | - [YARP](http://www.yarp.it) 30 | 31 | 32 | # 🔨 Build the suite 33 | Use the following commands to build, install and link the library. 34 | 35 | ### Build 36 | With `make` facilities: 37 | ```bash 38 | $ git clone https://github.com/robotology/visual-tracking-control 39 | $ cd visual-tracking-control 40 | $ mkdir build && cd build 41 | $ cmake -DBUILD_HAND_TRACKING=ON -DBUILD_VISUAL_SERVOING_CLIENT=ON -DBUILD_VISUAL_SERVOING_SERVER=ON .. 42 | $ make 43 | $ [sudo] make install 44 | ``` 45 | 46 | With IDE build tool facilities: 47 | ```bash 48 | $ git clone https://github.com/robotology/visual-tracking-control 49 | $ cd visual-tracking-control 50 | $ mkdir build && cd build 51 | $ cmake -DBUILD_HAND_TRACKING=ON -DBUILD_VISUAL_SERVOING_CLIENT=ON -DBUILD_VISUAL_SERVOING_SERVER=ON .. 52 | $ cmake --build . --target ALL_BUILD --config Release 53 | $ cmake --build . --target INSTALL --config Release 54 | ``` 55 | 56 | # 📝 API documentation and example code 57 | Doxygen-generated documentation is available [here](https://robotology.github.io/visual-tracking-control/doxygen/doc/html/index.html). 58 | 59 | 60 | ## 📑 References 61 | 62 | [1] C. Fantacci, U. Pattacini, V. Tikhanoff and L. Natale, "Visual end-effector tracking using a 3D model-aided particle filter for humanoid robot platforms", IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vancouver, BC, Canada, 2017. _arXiv preprint [arXiv:1703.04771](https://arxiv.org/abs/1703.04771)_. 63 | [2] C. Fantacci, G. Vezzani, U. Pattacini, V. Tikhanoff and L. Natale, "Markerless visual servoing on unknown objects for humanoid robot platforms", IEEE International Conference on Robotics and Automation (ICRA), Brisbane, AU, 2018. _arXiv preprint [arXiv:1710.04465](https://arxiv.org/abs/1710.04465)_. 64 | 65 | --- 66 | [![how-to-export-cpp-library](https://img.shields.io/badge/-Project%20Template-brightgreen.svg?style=flat&logo=data%3Aimage%2Fpng%3Bbase64%2CiVBORw0KGgoAAAANSUhEUgAAAEAAAAA9CAYAAAAd1W%2FBAAAABmJLR0QA%2FwD%2FAP%2BgvaeTAAAACXBIWXMAAAsTAAALEwEAmpwYAAAAB3RJTUUH4QEFECsmoylg4QAABRdJREFUaN7tmmuIVVUUx%2F%2F7OmpaaGP6oedkGJWNIWoFVqRZGkIPSrAQgqhEqSYxszeFUB%2FCAqcXUaSRZmZP6IFm42QEUWAjqT1EQ0dLHTMfaWajv76sM%2BxO59znuY%2Bcs2CYmXv33mud31577bX3WU5lEEDOueDvfpLGSBolaaiksyUNknRyqNs%2BSR2SfrKf1ZJaJG11zv1rzJoX4ETgYWAtpcuvwCvABQHcJMUlPevAi5KmxTTbKalN0hZJ2yRlvO%2BOlzTYvOScmP5fSrreOber1mZcQF9gU2j2dgDNwLgixmwE7ge%2BC415FDi%2FFt1%2BuWfkRuBqH1CJYw8B3vfG7wR61NLDn%2BoZt6IcHma%2F7%2FX0zEo6HpRi4KWeYWOTNswfz9OzoKpr3ov2s4HNnmHtwMAy6Vvk6VkPjKkWgInA5zm2r0eBulJn3P6%2FEdgZo2c%2F8BDQu9wP3Qg8DRyIMGJPFhCfAjOAUcAgwOXQ08%2BC3hSb8SMF5AyfANcG4Iteip7L9QMejNjeAlkEjLZ1n490Ah023g%2FAZ0AL8DWwAdgO%2FBnT9y%2Fgdm8CllggbI9ouxeYD4wsNtBcBXwcY8hGYGqo7xjKJyuAyZ6uQ%2Fb5fO%2BzEcCbMf23ANNzeZ6AYcA8oxeWbcDcIAGJWKOlANgCfGNesBR4Cpjqz15ocgIAr0Z4bE%2FgDhsvSt71kzJAAm7O4uJvABfnSmhKBNBY4PL8D4CYdqcBc4CDETp%2Fs3g2SDFGNRVoVCkARhQYlwJ5vgD7JgDLInTvzsT0mQd8BFyTTzrrnGstd84hqR5Y5321LJtNHrABks6V1FfSkVCzeuUxQweAl4Ah2WAAd5XDA4AzgOdCfVbmAe4G22GI2SXATnGFyBrg1rikw05vhcpwIGMBrI%2Bt3UnAMxYgw7Lc7I7Sf7oF0ajcYZ%2BdTBuA24oF4O%2FnS4ErI4w4E3irgLF22f5%2FMEe7r4AJ3vG7y8WBO4Fvs0T%2B8SEb7y4VgC%2B%2FW0QdGFLSC5hmsaRYWWNp7ikRoK%2FL4uLrbZZ7xnhqFwBHske3lZKelfSBc%2B5o6G6wQdJIuxMcIKnBu5FykrZL2iVpq6TVzrm2CMMHS5ouaYak8MPtlfS6pGbn3Ibw3WQYgKTm8LaSpOwHFgCXJHAC7A80AW0xupb4SzGf%2BUx6CeSzxmcBmQLT8Yl2VoiSDZbx9SgSbkUB%2BPKeHZwyMSn1YOBJ4HBM9tYMnFfqNVs1AQTSYQ8zDOgN3AOsi2n7jn%2FxkUTIqgUAuWSTbW3lyi67ANSpdmS3pIWSXnbOra2U0loB8IikJ4JXYJWUTI0AaA%2F260q%2F%2F8uom0sKIAWQAkgBpABSACmAFEAKIAWQAkgBpABSACmAFEB3kc5uBSD0wuUySVN8AB3dgEF%2FK7PdLWmVpOCV3dGMpCGSZkr6%2FliabeA44CagVdIeSXMl1XtNV0kaH%2B58VkQ1RiXklgQBjAYWW11hVLXbfVY2k3OgKfZ%2BvuYB2Bvk2THltIetYOOiYl2pAXgM%2BLkWAHh21dkktcaM2WolgD3DgbCUCDoceK3KAC7MUkO8A5gJ1Fci2DQBP1YCAHCSFWD9EtH3b3Pxy6sVdYdaZVZHEgA8Fw%2Fi0BcxfVqAyUCvklw84STjCuDDEgEMBxbGtPsDeAA4odb34D5WZt%2BeJ4AmK6PZHPHdQeBtYOz%2FNTEZCbwQU%2FaSq0x%2BEtCnqi6eMIxJWUrZAxd%2FPHjoY%2FZQYrnFHIvqh2zNj6uGTf8ARTOPo64fR94AAAAASUVORK5CYII%3D)](https://github.com/robotology/how-to-export-cpp-library) 67 | -------------------------------------------------------------------------------- /app/reaching-SIM.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | reaching-SIM 7 | This app allows the iCub_SIM to reach a point with the end-effector. Must be used with the hand-tracking app. 8 | 0.2.0 9 | 10 | 11 | Claudio Fantacci 12 | 13 | 14 | 15 | 16 | 17 | raching 18 | yarprun 19 | 20 | 21 | 22 | yarpview 23 | --name /view/cam_left --out /view/cam_left/click:o --x 20 --y 370 --RefreshTime 30 24 | yarprun 25 | 26 | 27 | 28 | yarpview 29 | --name /view/cam_right --out /view/cam_right/click:o --x 340 --y 370 --RefreshTime 30 30 | yarprun 31 | 32 | 33 | 34 | yarpview 35 | --name /view/cam_left/ee --x 20 --y 740 --RefreshTime 30 36 | yarprun 37 | 38 | 39 | 40 | yarpview 41 | --name /view/cam_right/ee --x 340 --y 740 --RefreshTime 30 42 | yarprun 43 | 44 | 45 | 46 | 47 | 48 | /hand-tracking/result/estimates:o 49 | /reaching/estimates:i 50 | tcp 51 | 52 | 53 | 54 | /icub/camcalib/left/out 55 | /view/cam_left 56 | tcp 57 | 58 | 59 | 60 | /icub/camcalib/left/out 61 | /reaching/cam_left/img:i 62 | tcp 63 | 64 | 65 | 66 | /reaching/cam_left/img:o 67 | /view/cam_left/ee 68 | tcp 69 | 70 | 71 | 72 | /view/cam_left/click:o 73 | /reaching/cam_left/click:i 74 | tcp 75 | 76 | 77 | 78 | /icub/camcalib/right/out 79 | /view/cam_right 80 | tcp 81 | 82 | 83 | 84 | /icub/camcalib/right/out 85 | /reaching/cam_right/img:i 86 | tcp 87 | 88 | 89 | 90 | /reaching/cam_right/img:o 91 | /view/cam_right/ee 92 | tcp 93 | 94 | 95 | 96 | /view/cam_right/click:o 97 | /reaching/cam_right/click:i 98 | tcp 99 | 100 | 101 | 102 | -------------------------------------------------------------------------------- /app/reaching.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | reaching 7 | This app allows the iCub to reach a point with the end-effector. Must be used with the hand-tracking app. 8 | 0.2.1 9 | 10 | 11 | Claudio Fantacci 12 | 13 | 14 | 15 | 16 | 17 | raching 18 | localhost 19 | 20 | 21 | 22 | yarpview 23 | --name /view/cam_left --out /view/cam_left/click:o --x 20 --y 370 --RefreshTime 30 24 | localhost 25 | 26 | 27 | 28 | yarpview 29 | --name /view/cam_right --out /view/cam_right/click:o --x 340 --y 370 --RefreshTime 30 30 | localhost 31 | 32 | 33 | 34 | yarpview 35 | --name /view/cam_left/ee --x 20 --y 740 --RefreshTime 30 36 | localhost 37 | 38 | 39 | 40 | yarpview 41 | --name /view/cam_right/ee --x 340 --y 740 --RefreshTime 30 42 | localhost 43 | 44 | 45 | 46 | yarpdatadumper 47 | localhost 48 | --name /datadumper/cam_left/x --txTime --rxTime --type bottle 49 | 50 | 51 | 52 | yarpdatadumper 53 | localhost 54 | --name /datadumper/cam_right/x --txTime --rxTime --type bottle 55 | 56 | 57 | 58 | 59 | 60 | /hand-tracking/result/estimates:o 61 | /reaching/estimates:i 62 | tcp 63 | 64 | 65 | 66 | /icub/cartesianController/right_arm/state:o 67 | /reaching/pose:i 68 | tcp 69 | 70 | 71 | 72 | /icub/camcalib/left/out 73 | /view/cam_left 74 | tcp 75 | 76 | 77 | 78 | /icub/camcalib/left/out 79 | /reaching/cam_left/img:i 80 | tcp 81 | 82 | 83 | 84 | /reaching/cam_left/img:o 85 | /view/cam_left/ee 86 | tcp 87 | 88 | 89 | 90 | /view/cam_left/click:o 91 | /reaching/cam_left/click:i 92 | tcp 93 | 94 | 95 | 96 | /view/cam_right/click:o 97 | /reaching/cam_right/click:i 98 | tcp 99 | 100 | 101 | 102 | /icub/camcalib/right/out 103 | /view/cam_right 104 | tcp 105 | 106 | 107 | 108 | /icub/camcalib/right/out 109 | /reaching/cam_right/img:i 110 | tcp 111 | 112 | 113 | 114 | /reaching/cam_right/img:o 115 | /view/cam_right/ee 116 | tcp 117 | 118 | 119 | 120 | /reaching/cam_left/x:o 121 | /datadumper/cam_left/x 122 | tcp 123 | 124 | 125 | 126 | /reaching/cam_right/x:o 127 | /datadumper/cam_right/x 128 | tcp 129 | 130 | 131 | 132 | -------------------------------------------------------------------------------- /cmake/AddInstallRPATHSupport.cmake: -------------------------------------------------------------------------------- 1 | #.rst: 2 | # AddInstallRPATHSupport 3 | # ---------------------- 4 | # 5 | # Add support to RPATH during installation to your project:: 6 | # 7 | # add_install_rpath_support([BIN_DIRS dir [dir]] 8 | # [LIB_DIRS dir [dir]] 9 | # [INSTALL_NAME_DIR [dir]] 10 | # [DEPENDS condition [condition]] 11 | # [USE_LINK_PATH]) 12 | # 13 | # Normally (depending on the platform) when you install a shared 14 | # library you can either specify its absolute path as the install name, 15 | # or leave just the library name itself. In the former case the library 16 | # will be correctly linked during run time by all executables and other 17 | # shared libraries, but it must not change its install location. This 18 | # is often the case for libraries installed in the system default 19 | # library directory (e.g. ``/usr/lib``). 20 | # In the latter case, instead, the library can be moved anywhere in the 21 | # file system but at run time the dynamic linker must be able to find 22 | # it. This is often accomplished by setting environmental variables 23 | # (i.e. ``LD_LIBRARY_PATH`` on Linux). 24 | # This procedure is usually not desirable for two main reasons: 25 | # 26 | # - by setting the variable you are changing the default behaviour 27 | # of the dynamic linker thus potentially breaking executables (not as 28 | # destructive as ``LD_PRELOAD``) 29 | # - the variable will be used only by applications spawned by the shell 30 | # and not by other processes. 31 | # 32 | # RPATH is aimed to solve the issues introduced by the second 33 | # installation method. Using run-path dependent libraries you can 34 | # create a directory structure containing executables and dependent 35 | # libraries that users can relocate without breaking it. 36 | # A run-path dependent library is a dependent library whose complete 37 | # install name is not known when the library is created. 38 | # Instead, the library specifies that the dynamic loader must resolve 39 | # the library’s install name when it loads the executable that depends 40 | # on the library. The executable or the other shared library will 41 | # hardcode in the binary itself the additional search directories 42 | # to be passed to the dynamic linker. This works great in conjunction 43 | # with relative paths. 44 | # This command will enable support to RPATH to your project. 45 | # It will enable the following things: 46 | # 47 | # - If the project builds shared libraries it will generate a run-path 48 | # enabled shared library, i.e. its install name will be resolved 49 | # only at run time. 50 | # - In all cases (building executables and/or shared libraries) 51 | # dependent shared libraries with RPATH support will be properly 52 | # 53 | # The command has the following parameters: 54 | # 55 | # Options: 56 | # - ``USE_LINK_PATH``: if passed the command will automatically adds to 57 | # the RPATH the path to all the dependent libraries. 58 | # 59 | # Arguments: 60 | # - ``BIN_DIRS`` list of directories when the targets (executable and 61 | # plugins) will be installed. 62 | # - ``LIB_DIRS`` list of directories to be added to the RPATH. These 63 | # directories will be added "relative" w.r.t. the ``BIN_DIRS`` and 64 | # ``LIB_DIRS``. 65 | # - ``INSTALL_NAME_DIR`` directory where the libraries will be installed. 66 | # This variable will be used only if ``CMAKE_SKIP_RPATH`` or 67 | # ``CMAKE_SKIP_INSTALL_RPATH`` is set to ``TRUE`` as it will set the 68 | # ``INSTALL_NAME_DIR`` on all targets 69 | # - ``DEPENDS`` list of conditions that should be ``TRUE`` to enable 70 | # RPATH, for example ``FOO; NOT BAR``. 71 | # 72 | # Note: see https://gitlab.kitware.com/cmake/cmake/issues/16589 for further 73 | # details. 74 | 75 | #======================================================================= 76 | # Copyright 2014 Istituto Italiano di Tecnologia (IIT) 77 | # @author Francesco Romano 78 | # 79 | # Distributed under the OSI-approved BSD License (the "License"); 80 | # see accompanying file Copyright.txt for details. 81 | # 82 | # This software is distributed WITHOUT ANY WARRANTY; without even the 83 | # implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 84 | # See the License for more information. 85 | #======================================================================= 86 | # (To distribute this file outside of CMake, substitute the full 87 | # License text for the above reference.) 88 | 89 | 90 | include(CMakeParseArguments) 91 | 92 | 93 | function(ADD_INSTALL_RPATH_SUPPORT) 94 | 95 | set(_options USE_LINK_PATH) 96 | set(_oneValueArgs INSTALL_NAME_DIR) 97 | set(_multiValueArgs BIN_DIRS 98 | LIB_DIRS 99 | DEPENDS) 100 | 101 | cmake_parse_arguments(_ARS "${_options}" 102 | "${_oneValueArgs}" 103 | "${_multiValueArgs}" 104 | "${ARGN}") 105 | 106 | # if either RPATH or INSTALL_RPATH is disabled 107 | # and the INSTALL_NAME_DIR variable is set, then hardcode the install name 108 | if(CMAKE_SKIP_RPATH OR CMAKE_SKIP_INSTALL_RPATH) 109 | if(DEFINED _ARS_INSTALL_NAME_DIR) 110 | set(CMAKE_INSTALL_NAME_DIR ${_ARS_INSTALL_NAME_DIR} PARENT_SCOPE) 111 | endif() 112 | endif() 113 | 114 | if (CMAKE_SKIP_RPATH OR (CMAKE_SKIP_INSTALL_RPATH AND CMAKE_SKIP_BUILD_RPATH)) 115 | return() 116 | endif() 117 | 118 | 119 | set(_rpath_available 1) 120 | if(DEFINED _ARS_DEPENDS) 121 | foreach(_dep ${_ARS_DEPENDS}) 122 | string(REGEX REPLACE " +" ";" _dep "${_dep}") 123 | if(NOT (${_dep})) 124 | set(_rpath_available 0) 125 | endif() 126 | endforeach() 127 | endif() 128 | 129 | if(_rpath_available) 130 | 131 | # Enable RPATH on OSX. 132 | set(CMAKE_MACOSX_RPATH TRUE PARENT_SCOPE) 133 | 134 | # Find system implicit lib directories 135 | set(_system_lib_dirs ${CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES}) 136 | if(EXISTS "/etc/debian_version") # is this a debian system ? 137 | if(CMAKE_LIBRARY_ARCHITECTURE) 138 | list(APPEND _system_lib_dirs "/lib/${CMAKE_LIBRARY_ARCHITECTURE}" 139 | "/usr/lib/${CMAKE_LIBRARY_ARCHITECTURE}") 140 | endif() 141 | endif() 142 | # This is relative RPATH for libraries built in the same project 143 | foreach(lib_dir ${_ARS_LIB_DIRS}) 144 | list(FIND _system_lib_dirs "${lib_dir}" isSystemDir) 145 | if("${isSystemDir}" STREQUAL "-1") 146 | foreach(bin_dir ${_ARS_LIB_DIRS} ${_ARS_BIN_DIRS}) 147 | file(RELATIVE_PATH _rel_path ${bin_dir} ${lib_dir}) 148 | if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin") 149 | list(APPEND CMAKE_INSTALL_RPATH "@loader_path/${_rel_path}") 150 | else() 151 | list(APPEND CMAKE_INSTALL_RPATH "\$ORIGIN/${_rel_path}") 152 | endif() 153 | endforeach() 154 | endif() 155 | endforeach() 156 | if(NOT "${CMAKE_INSTALL_RPATH}" STREQUAL "") 157 | list(REMOVE_DUPLICATES CMAKE_INSTALL_RPATH) 158 | endif() 159 | set(CMAKE_INSTALL_RPATH ${CMAKE_INSTALL_RPATH} PARENT_SCOPE) 160 | 161 | # add the automatically determined parts of the RPATH 162 | # which point to directories outside the build tree to the install RPATH 163 | set(CMAKE_INSTALL_RPATH_USE_LINK_PATH ${_ARS_USE_LINK_PATH} PARENT_SCOPE) 164 | 165 | endif() 166 | 167 | endfunction() 168 | -------------------------------------------------------------------------------- /cmake/AddUninstallTarget.cmake: -------------------------------------------------------------------------------- 1 | #.rst: 2 | # AddUninstallTarget 3 | # ------------------ 4 | # 5 | # Add the "uninstall" target for your project:: 6 | # 7 | # include(AddUninstallTarget) 8 | # 9 | # 10 | # will create a file cmake_uninstall.cmake in the build directory and add a 11 | # custom target uninstall that will remove the files installed by your package 12 | # (using install_manifest.txt) 13 | 14 | #============================================================================= 15 | # Copyright 2008-2013 Kitware, Inc. 16 | # Copyright 2013 iCub Facility, Istituto Italiano di Tecnologia 17 | # Authors: Daniele E. Domenichelli 18 | # 19 | # Distributed under the OSI-approved BSD License (the "License"); 20 | # see accompanying file Copyright.txt for details. 21 | # 22 | # This software is distributed WITHOUT ANY WARRANTY; without even the 23 | # implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 24 | # See the License for more information. 25 | #============================================================================= 26 | # (To distribute this file outside of CMake, substitute the full 27 | # License text for the above reference.) 28 | 29 | 30 | if(DEFINED __ADD_UNINSTALL_TARGET_INCLUDED) 31 | return() 32 | endif() 33 | set(__ADD_UNINSTALL_TARGET_INCLUDED TRUE) 34 | 35 | 36 | set(_filename ${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake) 37 | 38 | file(WRITE ${_filename} 39 | "if(NOT EXISTS \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\") 40 | message(WARNING \"Cannot find install manifest: \\\"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\\\"\") 41 | return() 42 | endif() 43 | 44 | file(READ \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\" files) 45 | string(STRIP \"\${files}\" files) 46 | string(REGEX REPLACE \"\\n\" \";\" files \"\${files}\") 47 | list(REVERSE files) 48 | foreach(file \${files}) 49 | message(STATUS \"Uninstalling: \$ENV{DESTDIR}\${file}\") 50 | if(EXISTS \"\$ENV{DESTDIR}\${file}\") 51 | execute_process( 52 | COMMAND \${CMAKE_COMMAND} -E remove \"\$ENV{DESTDIR}\${file}\" 53 | OUTPUT_VARIABLE rm_out 54 | RESULT_VARIABLE rm_retval) 55 | if(NOT \"\${rm_retval}\" EQUAL 0) 56 | message(FATAL_ERROR \"Problem when removing \\\"\$ENV{DESTDIR}\${file}\\\"\") 57 | endif() 58 | else() 59 | message(STATUS \"File \\\"\$ENV{DESTDIR}\${file}\\\" does not exist.\") 60 | endif() 61 | endforeach(file) 62 | ") 63 | 64 | if("${CMAKE_GENERATOR}" MATCHES "^(Visual Studio|Xcode)") 65 | set(_uninstall "UNINSTALL") 66 | else() 67 | set(_uninstall "uninstall") 68 | endif() 69 | add_custom_target(${_uninstall} COMMAND "${CMAKE_COMMAND}" -P "${_filename}") 70 | set_property(TARGET ${_uninstall} PROPERTY FOLDER "CMakePredefinedTargets") 71 | -------------------------------------------------------------------------------- /doc/main.dox: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * \author Claudio Fantacci 4 | * 5 | * \copyright BSD-3-Clause 6 | * 7 | * \section Description 8 | * The hand-tracking and reaching iCub apps. 9 | * Documentation is "Work In Progress". 10 | * 11 | * This file can be edited at doc/main.dox 12 | */ 13 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # BSD 3-Clause license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | option(BUILD_HAND_TRACKING "Build hand-tracking application" OFF) 11 | if(BUILD_HAND_TRACKING) 12 | add_subdirectory(hand-tracking) 13 | endif() 14 | 15 | option(BUILD_REACHING "Build reaching application" OFF) 16 | if(BUILD_REACHING) 17 | add_subdirectory(reaching) 18 | endif() 19 | 20 | add_subdirectory(visualservoing) 21 | -------------------------------------------------------------------------------- /src/hand-tracking/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # BSD 3-Clause license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | set(EXE_TARGET_NAME hand-tracking) 11 | 12 | # Dependencies 13 | find_package(BayesFilters REQUIRED) 14 | find_package(Eigen3 REQUIRED CONFIG) 15 | find_package(ICUB REQUIRED) 16 | find_package(OpenCV REQUIRED) 17 | find_package(SuperimposeMesh REQUIRED) 18 | find_package(YARP REQUIRED) 19 | 20 | LIST(APPEND CMAKE_MODULE_PATH ${YARP_MODULE_PATH} 21 | ${ICUB_MODULE_PATH}) 22 | 23 | include(YarpInstallationHelpers) 24 | 25 | # Application source and header files 26 | set(${EXE_TARGET_NAME}_HDR 27 | include/BrownianMotionPose.h 28 | include/DrawFwdKinPoses.h 29 | include/FwdKinModel.h 30 | include/GatePose.h 31 | include/iCubFwdKinModel.h 32 | include/iCubGatePose.h 33 | include/InitiCubArm.h 34 | include/PlayFwdKinModel.h 35 | include/PlayGatePose.h 36 | include/VisualProprioception.h 37 | include/VisualSIS.h 38 | include/VisualUpdateParticles.h) 39 | 40 | set(${EXE_TARGET_NAME}_SRC 41 | src/main.cpp 42 | src/BrownianMotionPose.cpp 43 | src/DrawFwdKinPoses.cpp 44 | src/FwdKinModel.cpp 45 | src/GatePose.cpp 46 | src/iCubFwdKinModel.cpp 47 | src/iCubGatePose.cpp 48 | src/InitiCubArm.cpp 49 | src/PlayFwdKinModel.cpp 50 | src/PlayGatePose.cpp 51 | src/VisualProprioception.cpp 52 | src/VisualSIS.cpp 53 | src/VisualUpdateParticles.cpp) 54 | 55 | set(${EXE_TARGET_NAME}_THRIFT_HDR 56 | thrift/visualsisparticlefilter.thrift) 57 | 58 | # Application target calls 59 | yarp_add_idl(${EXE_TARGET_NAME}_THRIFT_SRC ${${EXE_TARGET_NAME}_THRIFT_HDR}) 60 | 61 | add_executable(${EXE_TARGET_NAME} ${${EXE_TARGET_NAME}_HDR} ${${EXE_TARGET_NAME}_SRC} 62 | ${${EXE_TARGET_NAME}_THRIFT_SRC}) 63 | 64 | target_include_directories(${EXE_TARGET_NAME} PRIVATE "$" 65 | ${ICUB_INCLUDE_DIRS} 66 | ${YARP_INCLUDE_DIRS}) 67 | 68 | if(UNIX AND NOT APPLE) 69 | set(CMAKE_THREAD_PREFER_PTHREAD TRUE) 70 | set(THREADS_PREFER_PTHREAD_FLAG TRUE) 71 | find_package(Threads REQUIRED) 72 | 73 | target_link_libraries(${EXE_TARGET_NAME} PRIVATE BayesFilters::BayesFilters 74 | ctrlLib 75 | Eigen3::Eigen 76 | ${ICUB_LIBRARIES} 77 | iKin 78 | ${OpenCV_LIBS} 79 | SuperimposeMesh::SuperimposeMesh 80 | Threads::Threads 81 | ${YARP_LIBRARIES}) 82 | else() 83 | target_link_libraries(${EXE_TARGET_NAME} PRIVATE BayesFilters::BayesFilters 84 | ctrlLib 85 | Eigen3::Eigen 86 | ${ICUB_LIBRARIES} 87 | iKin 88 | ${OpenCV_LIBS} 89 | SuperimposeMesh::SuperimposeMesh 90 | ${YARP_LIBRARIES}) 91 | endif() 92 | 93 | 94 | # Custom command for RESOURCE/EXTRA files 95 | set(${EXE_TARGET_NAME}_APP 96 | ${CMAKE_CURRENT_SOURCE_DIR}/app/hand-tracking.xml 97 | ${CMAKE_CURRENT_SOURCE_DIR}/app/hand-tracking-BATCH.xml 98 | ${CMAKE_CURRENT_SOURCE_DIR}/app/hand-tracking-SIM.xml) 99 | 100 | set(${EXE_TARGET_NAME}_CONF 101 | ${CMAKE_CURRENT_SOURCE_DIR}/conf/config.ini) 102 | 103 | set(${EXE_TARGET_NAME}_SHADER_VERT 104 | ${CMAKE_CURRENT_SOURCE_DIR}/shader/shader_model.vert 105 | ${CMAKE_CURRENT_SOURCE_DIR}/shader/shader_background.vert) 106 | 107 | set(${EXE_TARGET_NAME}_SHADER_FRAG 108 | ${CMAKE_CURRENT_SOURCE_DIR}/shader/shader_model.frag 109 | ${CMAKE_CURRENT_SOURCE_DIR}/shader/shader_background.frag) 110 | 111 | set(${EXE_TARGET_NAME}_MESH 112 | ${CMAKE_CURRENT_SOURCE_DIR}/mesh/icub_right_arm/r_palm.obj 113 | ${CMAKE_CURRENT_SOURCE_DIR}/mesh/icub_right_arm/r_ail0.obj 114 | ${CMAKE_CURRENT_SOURCE_DIR}/mesh/icub_right_arm/r_ail1.obj 115 | ${CMAKE_CURRENT_SOURCE_DIR}/mesh/icub_right_arm/r_ail2.obj 116 | ${CMAKE_CURRENT_SOURCE_DIR}/mesh/icub_right_arm/r_ail3.obj 117 | ${CMAKE_CURRENT_SOURCE_DIR}/mesh/icub_right_arm/r_forearm.obj 118 | ${CMAKE_CURRENT_SOURCE_DIR}/mesh/icub_right_arm/r_indexbase.obj 119 | ${CMAKE_CURRENT_SOURCE_DIR}/mesh/icub_right_arm/r_ml0.obj 120 | ${CMAKE_CURRENT_SOURCE_DIR}/mesh/icub_right_arm/r_ml1.obj 121 | ${CMAKE_CURRENT_SOURCE_DIR}/mesh/icub_right_arm/r_ml2.obj 122 | ${CMAKE_CURRENT_SOURCE_DIR}/mesh/icub_right_arm/r_ml3.obj 123 | ${CMAKE_CURRENT_SOURCE_DIR}/mesh/icub_right_arm/r_tl0.obj 124 | ${CMAKE_CURRENT_SOURCE_DIR}/mesh/icub_right_arm/r_tl1.obj 125 | ${CMAKE_CURRENT_SOURCE_DIR}/mesh/icub_right_arm/r_tl2.obj 126 | ${CMAKE_CURRENT_SOURCE_DIR}/mesh/icub_right_arm/r_tl3.obj 127 | ${CMAKE_CURRENT_SOURCE_DIR}/mesh/icub_right_arm/r_tl4.obj) 128 | 129 | add_custom_command(TARGET ${EXE_TARGET_NAME} POST_BUILD 130 | COMMAND ${CMAKE_COMMAND} -E copy ${${EXE_TARGET_NAME}_SHADER_VERT} $ 131 | COMMAND ${CMAKE_COMMAND} -E copy ${${EXE_TARGET_NAME}_SHADER_FRAG} $ 132 | COMMAND ${CMAKE_COMMAND} -E copy ${${EXE_TARGET_NAME}_MESH} $) 133 | 134 | install(TARGETS ${EXE_TARGET_NAME} DESTINATION bin) 135 | install(FILES ${${EXE_TARGET_NAME}_APP} DESTINATION ${ICUBCONTRIB_APPLICATIONS_INSTALL_DIR}) 136 | install(FILES ${${EXE_TARGET_NAME}_CONF} DESTINATION ${ICUBCONTRIB_CONTEXTS_INSTALL_DIR}/${EXE_TARGET_NAME}) 137 | install(FILES ${${EXE_TARGET_NAME}_SHADER_VERT} DESTINATION ${ICUBCONTRIB_CONTEXTS_INSTALL_DIR}/${EXE_TARGET_NAME}/shader) 138 | install(FILES ${${EXE_TARGET_NAME}_SHADER_FRAG} DESTINATION ${ICUBCONTRIB_CONTEXTS_INSTALL_DIR}/${EXE_TARGET_NAME}/shader) 139 | install(FILES ${${EXE_TARGET_NAME}_MESH} DESTINATION ${ICUBCONTRIB_CONTEXTS_INSTALL_DIR}/${EXE_TARGET_NAME}/mesh) 140 | -------------------------------------------------------------------------------- /src/hand-tracking/app/hand-tracking-BATCH.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | hand-tracking-BATCH 7 | Offline visual hand tracking of the iCub hand by means of a 3D model-aided particle filter 8 | 0.5.0.0 9 | 10 | 11 | Claudio Fantacci 12 | 13 | 14 | 15 | 16 | 17 | hand-tracking 18 | --cam left 19 | localhost 20 | 21 | 22 | 23 | hand-tracking 24 | --cam right 25 | localhost 26 | 27 | 28 | 29 | yarpdataplayer 30 | --withExtraTimeCol 2 31 | localhost 32 | 33 | 34 | 35 | 36 | 37 | 38 | /icub/camcalib/left/out 39 | /hand-tracking/left/img:i 40 | mcast 41 | 42 | 43 | 44 | /icub/camcalib/right/out 45 | /hand-tracking/right/img:i 46 | mcast 47 | 48 | 49 | 50 | 51 | /icub/torso/state:o 52 | /hand-tracking/InitiCubArm/cam/left/torso:i 53 | tcp 54 | 55 | 56 | 57 | /icub/right_arm/state:o 58 | /hand-tracking/InitiCubArm/cam/left/right_arm:i 59 | tcp 60 | 61 | 62 | 63 | /icub/torso/state:o 64 | /hand-tracking/InitiCubArm/cam/right/torso:i 65 | tcp 66 | 67 | 68 | 69 | /icub/right_arm/state:o 70 | /hand-tracking/InitiCubArm/cam/right/right_arm:i 71 | tcp 72 | 73 | 74 | 75 | 76 | /icub/torso/state:o 77 | /hand-tracking/PlayFwdKinModel/left/torso:i 78 | tcp 79 | 80 | 81 | 82 | /icub/torso/state:o 83 | /hand-tracking/PlayFwdKinModel/right/torso:i 84 | tcp 85 | 86 | 87 | 88 | /icub/right_arm/state:o 89 | /hand-tracking/PlayFwdKinModel/left/right_arm:i 90 | tcp 91 | 92 | 93 | 94 | /icub/right_arm/state:o 95 | /hand-tracking/PlayFwdKinModel/right/right_arm:i 96 | tcp 97 | 98 | 99 | 100 | 101 | /icub/torso/state:o 102 | /hand-tracking/PlayGatePose/left/torso:i 103 | tcp 104 | 105 | 106 | 107 | /icub/torso/state:o 108 | /hand-tracking/PlayGatePose/right/torso:i 109 | tcp 110 | 111 | 112 | 113 | /icub/right_arm/state:o 114 | /hand-tracking/PlayGatePose/left/right_arm:i 115 | tcp 116 | 117 | 118 | 119 | /icub/right_arm/state:o 120 | /hand-tracking/PlayGatePose/right/right_arm:i 121 | tcp 122 | 123 | 124 | 125 | 126 | /icub/head/state:o 127 | /hand-tracking/VisualProprioception/left/head:i 128 | tcp 129 | 130 | 131 | 132 | /icub/head/state:o 133 | /hand-tracking/VisualProprioception/right/head:i 134 | tcp 135 | 136 | 137 | 138 | /icub/right_arm/state:o 139 | /hand-tracking/VisualProprioception/left/right_arm:i 140 | tcp 141 | 142 | 143 | 144 | /icub/right_arm/state:o 145 | /hand-tracking/VisualProprioception/right/right_arm:i 146 | tcp 147 | 148 | 149 | 150 | /icub/torso/state:o 151 | /hand-tracking/VisualProprioception/left/torso:i 152 | tcp 153 | 154 | 155 | 156 | /icub/torso/state:o 157 | /hand-tracking/VisualProprioception/right/torso:i 158 | tcp 159 | 160 | 161 | 162 | 163 | /icub/torso/state:o 164 | /hand-tracking/ResamplingWithPrior/InitiCubArm/cam/left/torso:i 165 | tcp 166 | 167 | 168 | 169 | /icub/right_arm/state:o 170 | /hand-tracking/ResamplingWithPrior/InitiCubArm/cam/left/right_arm:i 171 | tcp 172 | 173 | 174 | 175 | /icub/torso/state:o 176 | /hand-tracking/ResamplingWithPrior/InitiCubArm/cam/right/torso:i 177 | tcp 178 | 179 | 180 | 181 | /icub/right_arm/state:o 182 | /hand-tracking/ResamplingWithPrior/InitiCubArm/cam/right/right_arm:i 183 | tcp 184 | 185 | 186 | 187 | -------------------------------------------------------------------------------- /src/hand-tracking/app/hand-tracking-SIM.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | hand-tracking-SIM 7 | Visual hand tracking of the iCub_SIM hand by means of a 3D model-aided particle filter 8 | 0.5.0.0 9 | 10 | 11 | Claudio Fantacci 12 | 13 | 14 | 15 | 16 | 17 | hand-tracking 18 | --cam left 19 | localhost 20 | 21 | 22 | 23 | hand-tracking 24 | --cam right 25 | localhost 26 | 27 | 28 | 29 | 30 | 31 | 32 | /icubSim/cam/left 33 | /hand-tracking/left/img:i 34 | mcast 35 | 36 | 37 | 38 | /icubSim/cam/right 39 | /hand-tracking/right/img:i 40 | mcast 41 | 42 | 43 | 44 | 45 | /icubSim/torso/state:o 46 | /hand-tracking/InitiCubArm/cam/left/torso:i 47 | tcp 48 | 49 | 50 | 51 | /icubSim/right_arm/state:o 52 | /hand-tracking/InitiCubArm/cam/left/right_arm:i 53 | tcp 54 | 55 | 56 | 57 | /icubSim/torso/state:o 58 | /hand-tracking/InitiCubArm/cam/right/torso:i 59 | tcp 60 | 61 | 62 | 63 | /icubSim/right_arm/state:o 64 | /hand-tracking/InitiCubArm/cam/right/right_arm:i 65 | tcp 66 | 67 | 68 | 69 | 70 | /icubSim/head/state:o 71 | /hand-tracking/VisualProprioception/left/head:i 72 | tcp 73 | 74 | 75 | 76 | /icubSim/head/state:o 77 | /hand-tracking/VisualProprioception/right/head:i 78 | tcp 79 | 80 | 81 | 82 | /icubSim/right_arm/state:o 83 | /hand-tracking/VisualProprioception/left/right_arm:i 84 | tcp 85 | 86 | 87 | 88 | /icubSim/right_arm/state:o 89 | /hand-tracking/VisualProprioception/right/right_arm:i 90 | tcp 91 | 92 | 93 | 94 | /icubSim/torso/state:o 95 | /hand-tracking/VisualProprioception/left/torso:i 96 | tcp 97 | 98 | 99 | 100 | /icubSim/torso/state:o 101 | /hand-tracking/VisualProprioception/right/torso:i 102 | tcp 103 | 104 | 105 | 106 | 107 | /icubSim/torso/state:o 108 | /hand-tracking/ResamplingWithPrior/InitiCubArm/cam/left/torso:i 109 | tcp 110 | 111 | 112 | 113 | /icubSim/right_arm/state:o 114 | /hand-tracking/ResamplingWithPrior/InitiCubArm/cam/left/right_arm:i 115 | tcp 116 | 117 | 118 | 119 | /icubSim/torso/state:o 120 | /hand-tracking/ResamplingWithPrior/InitiCubArm/cam/right/torso:i 121 | tcp 122 | 123 | 124 | 125 | /icubSim/right_arm/state:o 126 | /hand-tracking/ResamplingWithPrior/InitiCubArm/cam/right/right_arm:i 127 | tcp 128 | 129 | 130 | 131 | -------------------------------------------------------------------------------- /src/hand-tracking/app/hand-tracking.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | hand-tracking 7 | Visual hand tracking of the iCub hand by means of a 3D model-aided particle filter 8 | 0.5.0.0 9 | 10 | 11 | Claudio Fantacci 12 | 13 | 14 | 15 | 16 | 17 | hand-tracking 18 | --cam left 19 | localhost 20 | 21 | 22 | 23 | hand-tracking 24 | --cam right 25 | localhost 26 | 27 | 28 | 29 | 30 | 31 | 32 | /icub/camcalib/left/out 33 | /hand-tracking/left/img:i 34 | mcast 35 | 36 | 37 | 38 | /icub/camcalib/right/out 39 | /hand-tracking/right/img:i 40 | mcast 41 | 42 | 43 | 44 | 45 | /icub/torso/state:o 46 | /hand-tracking/InitiCubArm/cam/left/torso:i 47 | tcp 48 | 49 | 50 | 51 | /icub/right_arm/state:o 52 | /hand-tracking/InitiCubArm/cam/left/right_arm:i 53 | tcp 54 | 55 | 56 | 57 | /icub/torso/state:o 58 | /hand-tracking/InitiCubArm/cam/right/torso:i 59 | tcp 60 | 61 | 62 | 63 | /icub/right_arm/state:o 64 | /hand-tracking/InitiCubArm/cam/right/right_arm:i 65 | tcp 66 | 67 | 68 | 69 | 70 | /icub/head/state:o 71 | /hand-tracking/VisualProprioception/left/head:i 72 | tcp 73 | 74 | 75 | 76 | /icub/head/state:o 77 | /hand-tracking/VisualProprioception/right/head:i 78 | tcp 79 | 80 | 81 | 82 | /icub/right_arm/state:o 83 | /hand-tracking/VisualProprioception/left/right_arm:i 84 | tcp 85 | 86 | 87 | 88 | /icub/right_arm/state:o 89 | /hand-tracking/VisualProprioception/right/right_arm:i 90 | tcp 91 | 92 | 93 | 94 | /icub/torso/state:o 95 | /hand-tracking/VisualProprioception/left/torso:i 96 | tcp 97 | 98 | 99 | 100 | /icub/torso/state:o 101 | /hand-tracking/VisualProprioception/right/torso:i 102 | tcp 103 | 104 | 105 | 106 | 107 | /icub/torso/state:o 108 | /hand-tracking/ResamplingWithPrior/InitiCubArm/cam/left/torso:i 109 | tcp 110 | 111 | 112 | 113 | /icub/right_arm/state:o 114 | /hand-tracking/ResamplingWithPrior/InitiCubArm/cam/left/right_arm:i 115 | tcp 116 | 117 | 118 | 119 | /icub/torso/state:o 120 | /hand-tracking/ResamplingWithPrior/InitiCubArm/cam/right/torso:i 121 | tcp 122 | 123 | 124 | 125 | /icub/right_arm/state:o 126 | /hand-tracking/ResamplingWithPrior/InitiCubArm/cam/right/right_arm:i 127 | tcp 128 | 129 | 130 | 131 | 132 | /hand-tracking/left/result/estimates:o 133 | /iCubProprioception/ExtCADSuperimposer/cam/left/hand/right_pose:i 134 | tcp 135 | 136 | 137 | 138 | /hand-tracking/right/result/estimates:o 139 | /iCubProprioception/ExtCADSuperimposer/cam/right/hand/right_pose:i 140 | tcp 141 | 142 | 143 | 144 | -------------------------------------------------------------------------------- /src/hand-tracking/conf/config.ini: -------------------------------------------------------------------------------- 1 | [PF] 2 | robot icub 3 | cam_sel left 4 | laterality right 5 | play 0.0 6 | num_particles 50 7 | gpu_count 1.0 8 | resample_prior 1.0 9 | gate_pose 0.0 10 | resolution_ratio 1.0 11 | 12 | [BROWNIANMOTION] 13 | q_xy 0.002 14 | q_z 0.002 15 | theta 0.5 16 | cone_angle 0.25 17 | seed 1.0 18 | 19 | [VISUALPROPRIOCEPTION] 20 | use_thumb 0.0 21 | use_forearm 0.0 22 | 23 | [VISUALUPDATEPARTICLES] 24 | likelihood_gain 0.001 25 | 26 | [GATEPOSE] 27 | gate_x 0.1 28 | gate_y 0.1 29 | gate_z 0.1 30 | gate_aperture 10.0 31 | gate_rotation 15.0 32 | 33 | [RESAMPLING] 34 | resample_ratio 0.2 35 | prior_ratio 0.5 36 | 37 | [FALLBACK] 38 | intrinsic_left (320, 240, 235.251, 160.871, 234.742, 124.055) 39 | intrinsic_right (320, 240, 234.667, 149.515, 233.927, 122.808) 40 | -------------------------------------------------------------------------------- /src/hand-tracking/include/BrownianMotionPose.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef BROWNIANMOTIONPOSE_H 9 | #define BROWNIANMOTIONPOSE_H 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | 17 | class BrownianMotionPose : public bfl::StateModel 18 | { 19 | public: 20 | BrownianMotionPose(const float q_xy, const float q_z, const float theta, const float cone_angle, const unsigned int seed) noexcept; 21 | 22 | BrownianMotionPose(const float q_xy, const float q_z, const float theta, const float cone_angle) noexcept; 23 | 24 | BrownianMotionPose() noexcept; 25 | 26 | BrownianMotionPose(const BrownianMotionPose& bm); 27 | 28 | BrownianMotionPose(BrownianMotionPose&& bm) noexcept; 29 | 30 | ~BrownianMotionPose() noexcept; 31 | 32 | BrownianMotionPose& operator=(const BrownianMotionPose& bm); 33 | 34 | BrownianMotionPose& operator=(BrownianMotionPose&& bm) noexcept; 35 | 36 | void propagate(const Eigen::Ref& cur_state, Eigen::Ref prop_state) override; 37 | 38 | void motion(const Eigen::Ref& cur_state, Eigen::Ref mot_state) override; 39 | 40 | Eigen::MatrixXf getNoiseSample(const int num) override; 41 | 42 | Eigen::MatrixXf getNoiseCovarianceMatrix() override { return Eigen::MatrixXf::Zero(1, 1); }; 43 | 44 | bool setProperty(const std::string& property) override { return false; }; 45 | 46 | protected: 47 | float q_xy_; /* Noise standard deviation for z 3D position */ 48 | float q_z_; /* Noise standard deviation for x-y 3D position */ 49 | float theta_; /* Noise standard deviation for axis-angle rotation */ 50 | float cone_angle_; /* Noise standard deviation for axis-angle axis cone */ 51 | 52 | Eigen::Vector4f cone_dir_; /* Cone direction of rotation. Fixed, left here for future implementation. */ 53 | 54 | std::mt19937_64 generator_; 55 | std::normal_distribution distribution_pos_xy_; 56 | std::normal_distribution distribution_pos_z_; 57 | std::normal_distribution distribution_theta_; 58 | std::uniform_real_distribution distribution_cone_; 59 | std::function gaussian_random_pos_xy_; 60 | std::function gaussian_random_pos_z_; 61 | std::function gaussian_random_theta_; 62 | std::function gaussian_random_cone_; 63 | 64 | void addAxisangleDisturbance(const Eigen::Ref& disturbance_vec, Eigen::Ref current_vec); 65 | }; 66 | 67 | #endif /* BROWNIANMOTIONPOSE_H */ 68 | -------------------------------------------------------------------------------- /src/hand-tracking/include/DrawFwdKinPoses.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef DRAWFWDKINPOSES_H 9 | #define DRAWFWDKINPOSES_H 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | namespace bfl { 18 | class DrawFwdKinPoses; 19 | } 20 | 21 | 22 | class bfl::DrawFwdKinPoses : public bfl::PFPrediction 23 | { 24 | public: 25 | DrawFwdKinPoses() noexcept; 26 | 27 | DrawFwdKinPoses(DrawFwdKinPoses&& pf_prediction) noexcept; 28 | 29 | ~DrawFwdKinPoses() noexcept; 30 | 31 | DrawFwdKinPoses& operator=(DrawFwdKinPoses&& pf_prediction) noexcept; 32 | 33 | 34 | StateModel& getStateModel() override; 35 | 36 | void setStateModel(std::unique_ptr state_model) override; 37 | 38 | ExogenousModel& getExogenousModel() override; 39 | 40 | void setExogenousModel(std::unique_ptr exogenous_model) override; 41 | 42 | protected: 43 | void predictStep(const Eigen::Ref& prev_states, const Eigen::Ref& prev_weights, 44 | Eigen::Ref pred_states, Eigen::Ref pred_weights) override; 45 | 46 | std::unique_ptr state_model_; 47 | 48 | std::unique_ptr exogenous_model_; 49 | }; 50 | 51 | #endif /* DRAWFWDKINPOSES_H */ 52 | -------------------------------------------------------------------------------- /src/hand-tracking/include/FwdKinModel.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef FWDKINMODEL_H 9 | #define FWDKINMODEL_H 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | 19 | class FwdKinModel : public bfl::ExogenousModel 20 | { 21 | public: 22 | FwdKinModel() noexcept; 23 | 24 | ~FwdKinModel() noexcept; 25 | 26 | void propagate(const Eigen::Ref& cur_state, Eigen::Ref prop_state) override; 27 | 28 | Eigen::MatrixXf getExogenousMatrix() override; 29 | 30 | bool setProperty(const std::string& property) override; 31 | 32 | protected: 33 | virtual Eigen::VectorXd readPose() = 0; 34 | 35 | bool initialize_delta_ = true; 36 | bool setDeltaMotion(); 37 | 38 | private: 39 | Eigen::VectorXd prev_ee_pose_ = Eigen::VectorXd::Zero(7); 40 | Eigen::VectorXd delta_hand_pose_ = Eigen::VectorXd::Zero(6); 41 | double delta_angle_ = 0.0; 42 | }; 43 | 44 | #endif /* FWDKINMODEL_H */ 45 | -------------------------------------------------------------------------------- /src/hand-tracking/include/GatePose.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef GATEPOSE_H 9 | #define GATEPOSE_H 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | 17 | class GatePose : public bfl::PFVisualCorrectionDecorator 18 | { 19 | public: 20 | GatePose(std::unique_ptr visual_correction, 21 | const double gate_x, const double gate_y, const double gate_z, 22 | const double gate_rotation, 23 | const double gate_aperture) noexcept; 24 | 25 | GatePose(std::unique_ptr visual_correction) noexcept; 26 | 27 | ~GatePose() noexcept; 28 | 29 | void innovation(const Eigen::Ref& pred_states, cv::InputArray measurements, Eigen::Ref innovations) override; 30 | 31 | double likelihood(const Eigen::Ref& innovations) override; 32 | 33 | bfl::VisualObservationModel& getVisualObservationModel() override; 34 | 35 | void setVisualObservationModel(std::unique_ptr visual_observation_model) override; 36 | 37 | protected: 38 | void correctStep(const Eigen::Ref& pred_states, const Eigen::Ref& pred_weights, cv::InputArray measurements, 39 | Eigen::Ref cor_states, Eigen::Ref cor_weights) override; 40 | 41 | virtual Eigen::VectorXd readPose() = 0; 42 | 43 | bool isInsideEllipsoid(const Eigen::Ref& state); 44 | 45 | bool isWithinRotation(float rot_angle); 46 | 47 | bool isInsideCone(const Eigen::Ref& state); 48 | 49 | private: 50 | double gate_x_; 51 | double gate_y_; 52 | double gate_z_; 53 | double gate_aperture_; 54 | double gate_rotation_; 55 | 56 | Eigen::VectorXd ee_pose_; 57 | }; 58 | 59 | #endif /* GATEPOSE_H */ 60 | -------------------------------------------------------------------------------- /src/hand-tracking/include/InitiCubArm.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef INITICUBARM_H 9 | #define INITICUBARM_H 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | 19 | class InitiCubArm : public bfl::Initialization 20 | { 21 | public: 22 | InitiCubArm(const yarp::os::ConstString& port_prefix, const yarp::os::ConstString& cam_sel, const yarp::os::ConstString& laterality) noexcept; 23 | 24 | InitiCubArm(const yarp::os::ConstString& cam_sel, const yarp::os::ConstString& laterality) noexcept; 25 | 26 | ~InitiCubArm() noexcept; 27 | 28 | void initialize(Eigen::Ref state, Eigen::Ref weight) override; 29 | 30 | private: 31 | iCub::iKin::iCubArm icub_kin_arm_; 32 | iCub::iKin::iCubFinger icub_kin_finger_[3]; 33 | yarp::os::BufferedPort port_torso_enc_; 34 | yarp::os::BufferedPort port_arm_enc_; 35 | 36 | yarp::sig::Vector readTorso(); 37 | 38 | yarp::sig::Vector readRootToEE(); 39 | }; 40 | 41 | #endif /* INITICUBARM_H */ 42 | -------------------------------------------------------------------------------- /src/hand-tracking/include/PlayFwdKinModel.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef PLAYFWDKINMODEL_H 9 | #define PLAYFWDKINMODEL_H 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | 20 | class PlayFwdKinModel : public FwdKinModel 21 | { 22 | public: 23 | PlayFwdKinModel(const yarp::os::ConstString& robot, const yarp::os::ConstString& laterality, const yarp::os::ConstString& port_prefix) noexcept; 24 | 25 | ~PlayFwdKinModel() noexcept; 26 | 27 | bool setProperty(const std::string& property) override; 28 | 29 | protected: 30 | Eigen::VectorXd readPose() override; 31 | 32 | yarp::sig::Vector readTorso(); 33 | 34 | yarp::sig::Vector readRootToEE(); 35 | 36 | yarp::os::BufferedPort port_torso_enc_; 37 | yarp::os::BufferedPort port_arm_enc_; 38 | iCub::iKin::iCubArm icub_kin_arm_; 39 | 40 | private: 41 | yarp::os::ConstString ID_ = "PlayFwdKinModel"; 42 | yarp::os::ConstString log_ID_ = "[" + ID_ + "]"; 43 | 44 | yarp::os::ConstString robot_; 45 | yarp::os::ConstString laterality_; 46 | yarp::os::ConstString port_prefix_; 47 | }; 48 | 49 | #endif /* PLAYFWDKINMODEL_H */ 50 | -------------------------------------------------------------------------------- /src/hand-tracking/include/PlayGatePose.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef PLAYGATEPOSE_H 9 | #define PLAYGATEPOSE_H 10 | 11 | #include "GatePose.h" 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | 19 | class PlayGatePose : public GatePose 20 | { 21 | public: 22 | /* Constructor */ 23 | PlayGatePose(std::unique_ptr visual_correction, 24 | const double gate_x, const double gate_y, const double gate_z, 25 | const double gate_aperture, 26 | const double gate_rotation, 27 | const yarp::os::ConstString& robot, const yarp::os::ConstString& laterality, const yarp::os::ConstString& port_prefix) noexcept; 28 | 29 | PlayGatePose(std::unique_ptr visual_correction, 30 | const yarp::os::ConstString& robot, const yarp::os::ConstString& laterality, const yarp::os::ConstString& port_prefix) noexcept; 31 | 32 | /* Destructor */ 33 | ~PlayGatePose() noexcept override; 34 | 35 | protected: 36 | yarp::os::BufferedPort port_torso_enc_; 37 | yarp::os::BufferedPort port_arm_enc_; 38 | iCub::iKin::iCubArm icub_kin_arm_; 39 | 40 | Eigen::VectorXd readPose() override; 41 | 42 | yarp::sig::Vector readRootToEE(); 43 | 44 | yarp::sig::Vector readTorso(); 45 | 46 | private: 47 | const yarp::os::ConstString ID_ = "PlayGatePose"; 48 | const yarp::os::ConstString log_ID_ = "[" + ID_ + "]"; 49 | 50 | yarp::os::ConstString robot_; 51 | yarp::os::ConstString laterality_; 52 | yarp::os::ConstString port_prefix_; 53 | }; 54 | 55 | #endif /* PLAYGATEPOSE_H */ 56 | -------------------------------------------------------------------------------- /src/hand-tracking/include/VisualProprioception.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef VISUALPROPRIOCEPTION_H 9 | #define VISUALPROPRIOCEPTION_H 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | 27 | class VisualProprioception : public bfl::VisualObservationModel 28 | { 29 | public: 30 | VisualProprioception(const bool use_thumb, 31 | const bool use_forearm, 32 | const int num_images, 33 | const double resolution_ratio, 34 | const yarp::os::ConstString& cam_sel, 35 | const yarp::os::ConstString& laterality, 36 | const yarp::os::ConstString& context); 37 | 38 | VisualProprioception(const VisualProprioception& proprio); 39 | 40 | VisualProprioception(VisualProprioception&& proprio) noexcept; 41 | 42 | ~VisualProprioception() noexcept; 43 | 44 | VisualProprioception& operator=(const VisualProprioception& proprio); 45 | 46 | VisualProprioception& operator=(VisualProprioception&& proprio) noexcept; 47 | 48 | void observe(const Eigen::Ref& cur_states, cv::OutputArray observations) override; 49 | 50 | bool setProperty(const std::string property) override; 51 | 52 | int getOGLTilesNumber(); 53 | int getOGLTilesRows(); 54 | int getOGLTilesCols(); 55 | 56 | unsigned int getCamWidth(); 57 | unsigned int getCamHeight(); 58 | 59 | float getCamFx(); 60 | float getCamFy(); 61 | float getCamCx(); 62 | float getCamCy(); 63 | 64 | protected: 65 | yarp::os::ConstString log_ID_ = "[VisualProprioception]"; 66 | 67 | /* ICUB */ 68 | yarp::os::ConstString laterality_; 69 | yarp::dev::PolyDriver drv_gaze_; 70 | yarp::dev::IGazeControl* itf_gaze_; 71 | iCub::iKin::iCubArm icub_arm_; 72 | iCub::iKin::iCubFinger icub_kin_finger_[3]; 73 | iCub::iKin::iCubEye icub_kin_eye_; 74 | yarp::os::BufferedPort port_head_enc_; 75 | yarp::os::BufferedPort port_torso_enc_; 76 | yarp::os::BufferedPort port_arm_enc_; 77 | yarp::dev::PolyDriver drv_right_hand_analog_; 78 | yarp::dev::IAnalogSensor * itf_right_hand_analog_; 79 | yarp::sig::Matrix right_hand_analogs_bounds_; 80 | 81 | yarp::sig::Matrix getInvertedH(const double a, const double d, const double alpha, const double offset, const double q); 82 | 83 | bool openGazeController(); 84 | 85 | bool openAnalogs(); 86 | bool closeAnalogs(); 87 | bool analogs_ = false; 88 | 89 | bool setiCubParams(); 90 | 91 | void setArmJoints(const yarp::sig::Vector& q); 92 | 93 | void setArmJoints(const yarp::sig::Vector& q, const yarp::sig::Vector& analogs, const yarp::sig::Matrix& analog_bounds); 94 | 95 | void getModelPose(const Eigen::Ref& cur_states, std::vector& hand_poses); 96 | 97 | yarp::sig::Vector readArmEncoders(); 98 | yarp::sig::Vector readTorso(); 99 | yarp::sig::Vector readRootToFingers(); 100 | yarp::sig::Vector readRootToEye(const yarp::os::ConstString cam_sel); 101 | /* **** */ 102 | 103 | yarp::os::ConstString cam_sel_; 104 | double cam_x_[3]; 105 | double cam_o_[4]; 106 | unsigned int cam_width_; 107 | unsigned int cam_height_; 108 | float cam_fx_; 109 | float cam_cx_; 110 | float cam_fy_; 111 | float cam_cy_; 112 | double resolution_ratio_; 113 | 114 | bool use_thumb_; 115 | bool use_forearm_; 116 | SICAD::ModelPathContainer cad_obj_; 117 | SICAD* si_cad_; 118 | int ogl_tiles_rows_; 119 | int ogl_tiles_cols_; 120 | 121 | bool file_found(const yarp::os::ConstString& file); 122 | 123 | }; 124 | 125 | #endif /* VISUALPROPRIOCEPTION_H */ 126 | -------------------------------------------------------------------------------- /src/hand-tracking/include/VisualSIS.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef VISUALSIS_H 9 | #define VISUALSIS_H 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | 33 | class VisualSIS: public bfl::VisualParticleFilter, 34 | public VisualSISParticleFilterIDL 35 | { 36 | public: 37 | VisualSIS(const yarp::os::ConstString& cam_sel, 38 | const int img_width, const int img_height, 39 | const int num_particles, 40 | const double resample_ratio); 41 | 42 | ~VisualSIS() noexcept; 43 | 44 | protected: 45 | void initialization() override; 46 | 47 | void filteringStep() override; 48 | 49 | void getResult() override; 50 | 51 | bool runCondition() override { return true; }; 52 | 53 | 54 | yarp::os::ConstString cam_sel_; 55 | int img_width_; 56 | int img_height_; 57 | int num_particles_; 58 | unsigned int descriptor_length_; 59 | double resample_ratio_; 60 | 61 | const int block_size_ = 16; 62 | const int bin_number_ = 9; 63 | 64 | cv::Ptr cuda_hog_; 65 | 66 | 67 | yarp::os::BufferedPort port_estimates_out_; 68 | yarp::os::BufferedPort> port_image_in_; 69 | 70 | 71 | yarp::os::Port port_rpc_command_; 72 | bool attach(yarp::os::Port &source); 73 | bool setCommandPort(); 74 | 75 | 76 | bool run_filter() override; 77 | 78 | bool reset_filter() override; 79 | 80 | bool stop_filter() override; 81 | 82 | bool skip_step(const std::string& what_step, const bool status) override; 83 | 84 | bool use_analogs(const bool status) override; 85 | 86 | std::vector get_info() override; 87 | 88 | bool quit() override; 89 | 90 | 91 | bool set_estimates_extraction_method(const std::string& method) override; 92 | 93 | bool set_mobile_average_window(const int16_t window) override; 94 | 95 | private: 96 | Eigen::MatrixXf pred_particle_; 97 | Eigen::VectorXf pred_weight_; 98 | 99 | Eigen::MatrixXf cor_particle_; 100 | Eigen::VectorXf cor_weight_; 101 | 102 | 103 | bfl::EstimatesExtraction estimate_extraction_; 104 | 105 | 106 | bool init_img_in_ = false; 107 | yarp::sig::ImageOf img_in_; 108 | }; 109 | 110 | #endif /* VISUALSIS_H */ 111 | -------------------------------------------------------------------------------- /src/hand-tracking/include/VisualUpdateParticles.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef VISUALUPDATEPARTICLES_H 9 | #define VISUALUPDATEPARTICLES_H 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | 23 | class VisualUpdateParticles : public bfl::PFVisualCorrection 24 | { 25 | public: 26 | VisualUpdateParticles(std::unique_ptr observation_model) noexcept; 27 | 28 | VisualUpdateParticles(std::unique_ptr observation_model, const double likelihood_gain) noexcept; 29 | 30 | VisualUpdateParticles(std::unique_ptr observation_model, const double likelihood_gain, const int num_cuda_stream) noexcept; 31 | 32 | ~VisualUpdateParticles() noexcept; 33 | 34 | void innovation(const Eigen::Ref& pred_states, cv::InputArray measurements, Eigen::Ref innovations) override; 35 | 36 | double likelihood(const Eigen::Ref& innovations) override; 37 | 38 | bfl::VisualObservationModel& getVisualObservationModel() override; 39 | 40 | void setVisualObservationModel(std::unique_ptr visual_observation_model) override; 41 | 42 | protected: 43 | void correctStep(const Eigen::Ref& pred_states, const Eigen::Ref& pred_weights, cv::InputArray measurements, 44 | Eigen::Ref cor_states, Eigen::Ref cor_weights) override; 45 | 46 | std::unique_ptr observation_model_; 47 | double likelihood_gain_; 48 | 49 | cv::Ptr cuda_hog_; 50 | 51 | const int num_cuda_stream_; 52 | const int num_img_stream_; 53 | std::vector cuda_stream_; 54 | std::vector hand_rendered_; 55 | std::vector cuda_img_; 56 | std::vector cuda_img_alpha_; 57 | std::vector cuda_descriptors_; 58 | std::vector cpu_descriptors_; 59 | }; 60 | 61 | #endif /* VISUALUPDATEPARTICLES_H */ 62 | -------------------------------------------------------------------------------- /src/hand-tracking/include/iCubFwdKinModel.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef ICUBFWDKINMODEL_H 9 | #define ICUBFWDKINMODEL_H 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | 20 | class iCubFwdKinModel : public FwdKinModel 21 | { 22 | public: 23 | iCubFwdKinModel(const yarp::os::ConstString& robot, const yarp::os::ConstString& laterality, const yarp::os::ConstString& port_prefix); 24 | 25 | ~iCubFwdKinModel() noexcept; 26 | 27 | bool setProperty(const std::string& property) override; 28 | 29 | protected: 30 | Eigen::VectorXd readPose() override; 31 | 32 | yarp::sig::Vector readTorso(); 33 | 34 | yarp::sig::Vector readRootToEE(); 35 | 36 | yarp::dev::PolyDriver drv_arm_enc_; 37 | yarp::dev::PolyDriver drv_torso_enc_; 38 | yarp::dev::IEncoders * itf_arm_enc_; 39 | yarp::dev::IEncoders * itf_torso_enc_; 40 | iCub::iKin::iCubArm icub_kin_arm_; 41 | 42 | private: 43 | yarp::os::ConstString ID_ = "iCubFwdKinModel"; 44 | yarp::os::ConstString log_ID_ = "[" + ID_ + "]"; 45 | 46 | yarp::os::ConstString robot_; 47 | yarp::os::ConstString laterality_; 48 | yarp::os::ConstString port_prefix_; 49 | }; 50 | 51 | #endif /* ICUBFWDKINMODEL_H */ 52 | -------------------------------------------------------------------------------- /src/hand-tracking/include/iCubGatePose.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef ICUBGATEPOSE_H 9 | #define ICUBGATEPOSE_H 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | 20 | class iCubGatePose : public GatePose 21 | { 22 | public: 23 | iCubGatePose(std::unique_ptr visual_correction, 24 | const double gate_x, const double gate_y, const double gate_z, 25 | const double gate_aperture, 26 | const double gate_rotation, 27 | const yarp::os::ConstString& robot, const yarp::os::ConstString& laterality, const yarp::os::ConstString& port_prefix); 28 | 29 | iCubGatePose(std::unique_ptr visual_correction, 30 | const yarp::os::ConstString& robot, const yarp::os::ConstString& laterality, const yarp::os::ConstString& port_prefix); 31 | 32 | ~iCubGatePose() noexcept override; 33 | 34 | protected: 35 | yarp::dev::PolyDriver drv_arm_enc_; 36 | yarp::dev::IEncoders * itf_arm_enc_; 37 | yarp::dev::PolyDriver drv_torso_enc_; 38 | yarp::dev::IEncoders * itf_torso_enc_; 39 | 40 | iCub::iKin::iCubArm icub_kin_arm_; 41 | 42 | Eigen::VectorXd readPose() override; 43 | 44 | yarp::sig::Vector readRootToEE(); 45 | 46 | yarp::sig::Vector readTorso(); 47 | 48 | private: 49 | const yarp::os::ConstString ID_ = "iCubGatePose"; 50 | const yarp::os::ConstString log_ID_ = "[" + ID_ + "]"; 51 | 52 | yarp::os::ConstString robot_; 53 | yarp::os::ConstString laterality_; 54 | yarp::os::ConstString port_prefix_; 55 | }; 56 | 57 | #endif /* ICUBGATEPOSE_H */ 58 | -------------------------------------------------------------------------------- /src/hand-tracking/shader/shader_background.frag: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #version 330 core 9 | 10 | in vec3 ourColor; 11 | in vec2 TexCoord; 12 | 13 | out vec4 color; 14 | 15 | uniform sampler2D ourTexture; 16 | 17 | void main() 18 | { 19 | color = texture(ourTexture, TexCoord); 20 | } 21 | -------------------------------------------------------------------------------- /src/hand-tracking/shader/shader_background.vert: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #version 330 core 9 | 10 | layout (location = 0) in vec2 position; 11 | layout (location = 1) in vec3 color; 12 | layout (location = 2) in vec2 texCoord; 13 | 14 | out vec3 ourColor; 15 | out vec2 TexCoord; 16 | 17 | uniform mat4 projection; 18 | 19 | void main() 20 | { 21 | //gl_Position = vec4(position, 0.99, 1.0f); 22 | gl_Position = projection * vec4(position, -99999.99, 1.0f); 23 | ourColor = color; 24 | TexCoord = vec2(texCoord.x, 1.0f - texCoord.y); 25 | } 26 | -------------------------------------------------------------------------------- /src/hand-tracking/shader/shader_model.frag: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #version 330 core 9 | 10 | out vec4 color; 11 | 12 | void main() 13 | { 14 | // color = vec4(1.0f, 0.5f, 0.2f, 1.0f); // RGB orange-like color 15 | color = vec4(0.2f, 0.5f, 1.0f, 1.0f); // BGR orange-like color 16 | } 17 | -------------------------------------------------------------------------------- /src/hand-tracking/shader/shader_model.vert: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #version 330 core 9 | 10 | layout (location = 0) in vec3 position; 11 | layout (location = 1) in vec3 normal; 12 | layout (location = 2) in vec2 texCoords; 13 | 14 | out vec2 TexCoords; 15 | 16 | uniform mat4 model; 17 | uniform mat4 view; 18 | uniform mat4 projection; 19 | 20 | void main() 21 | { 22 | // gl_Position = projection * view * model * vec4(position/1000.0f, 1.0f); 23 | gl_Position = projection * view * model * vec4(position, 1.0f); 24 | TexCoords = texCoords; 25 | } 26 | -------------------------------------------------------------------------------- /src/hand-tracking/src/BrownianMotionPose.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | using namespace Eigen; 15 | 16 | 17 | BrownianMotionPose::BrownianMotionPose(const float q_xy, const float q_z, const float theta, const float cone_angle, const unsigned int seed) noexcept : 18 | q_xy_(q_xy), 19 | q_z_(q_z), 20 | theta_(theta * (M_PI/180.0)), 21 | cone_angle_(cone_angle * (M_PI/180.0)), 22 | cone_dir_(Vector4f(0.0, 0.0, 1.0, 0.0)), 23 | generator_(std::mt19937_64(seed)), 24 | distribution_pos_xy_(std::normal_distribution(0.0, q_xy)), 25 | distribution_pos_z_(std::normal_distribution(0.0, q_z)), 26 | distribution_theta_(std::normal_distribution(0.0, theta_)), 27 | distribution_cone_(std::uniform_real_distribution(0.0, 1.0)), 28 | gaussian_random_pos_xy_([&] { return (distribution_pos_xy_)(generator_); }), 29 | gaussian_random_pos_z_([&] { return (distribution_pos_z_)(generator_); }), 30 | gaussian_random_theta_([&] { return (distribution_theta_)(generator_); }), 31 | gaussian_random_cone_([&] { return (distribution_cone_)(generator_); }) { } 32 | 33 | 34 | BrownianMotionPose::BrownianMotionPose(const float q_xy, const float q_z, const float theta, const float cone_angle) noexcept : 35 | BrownianMotionPose(q_xy, q_z, theta, cone_angle, 1) { } 36 | 37 | 38 | BrownianMotionPose::BrownianMotionPose() noexcept : 39 | BrownianMotionPose(0.005, 0.005, 3.0, 2.5, 1) { } 40 | 41 | 42 | BrownianMotionPose::BrownianMotionPose(const BrownianMotionPose& brown) : 43 | q_xy_(brown.q_xy_), 44 | q_z_(brown.q_z_), 45 | theta_(brown.theta_), 46 | cone_angle_(brown.cone_angle_), 47 | cone_dir_(brown.cone_dir_), 48 | generator_(brown.generator_), 49 | distribution_pos_xy_(brown.distribution_pos_xy_), 50 | distribution_pos_z_(brown.distribution_pos_z_), 51 | distribution_theta_(brown.distribution_theta_), 52 | distribution_cone_(brown.distribution_cone_), 53 | gaussian_random_pos_xy_(brown.gaussian_random_pos_xy_), 54 | gaussian_random_pos_z_(brown.gaussian_random_pos_z_), 55 | gaussian_random_theta_(brown.gaussian_random_theta_), 56 | gaussian_random_cone_(brown.gaussian_random_cone_) { } 57 | 58 | 59 | BrownianMotionPose::BrownianMotionPose(BrownianMotionPose&& brown) noexcept : 60 | q_xy_(brown.q_xy_), 61 | theta_(brown.theta_), 62 | cone_angle_(brown.cone_angle_), 63 | cone_dir_(std::move(brown.cone_dir_)), 64 | generator_(std::move(brown.generator_)), 65 | distribution_pos_xy_(std::move(brown.distribution_pos_xy_)), 66 | distribution_pos_z_(std::move(brown.distribution_pos_z_)), 67 | distribution_theta_(std::move(brown.distribution_theta_)), 68 | distribution_cone_(std::move(brown.distribution_cone_)), 69 | gaussian_random_pos_xy_(std::move(brown.gaussian_random_pos_xy_)), 70 | gaussian_random_pos_z_(std::move(brown.gaussian_random_pos_z_)), 71 | gaussian_random_theta_(std::move(brown.gaussian_random_theta_)), 72 | gaussian_random_cone_(std::move(brown.gaussian_random_cone_)) 73 | { 74 | brown.q_xy_ = 0.0; 75 | brown.q_z_ = 0.0; 76 | brown.theta_ = 0.0; 77 | brown.cone_angle_ = 0.0; 78 | } 79 | 80 | 81 | BrownianMotionPose::~BrownianMotionPose() noexcept { } 82 | 83 | 84 | BrownianMotionPose& BrownianMotionPose::operator=(const BrownianMotionPose& brown) 85 | { 86 | BrownianMotionPose tmp(brown); 87 | *this = std::move(tmp); 88 | 89 | return *this; 90 | } 91 | 92 | 93 | BrownianMotionPose& BrownianMotionPose::operator=(BrownianMotionPose&& brown) noexcept 94 | { 95 | q_xy_ = brown.q_xy_; 96 | q_z_ = brown.q_z_; 97 | theta_ = brown.theta_; 98 | cone_angle_ = brown.cone_angle_; 99 | cone_dir_ = std::move(brown.cone_dir_); 100 | 101 | generator_ = std::move(brown.generator_); 102 | distribution_pos_xy_ = std::move(brown.distribution_pos_xy_); 103 | distribution_pos_z_ = std::move(brown.distribution_pos_z_); 104 | distribution_theta_ = std::move(brown.distribution_theta_); 105 | distribution_cone_ = std::move(brown.distribution_cone_); 106 | gaussian_random_pos_xy_ = std::move(brown.gaussian_random_pos_xy_); 107 | gaussian_random_pos_z_ = std::move(brown.gaussian_random_pos_z_); 108 | gaussian_random_theta_ = std::move(brown.gaussian_random_theta_); 109 | gaussian_random_cone_ = std::move(brown.gaussian_random_cone_); 110 | 111 | brown.q_xy_ = 0.0; 112 | brown.q_z_ = 0.0; 113 | brown.theta_ = 0.0; 114 | brown.cone_angle_ = 0.0; 115 | 116 | return *this; 117 | } 118 | 119 | 120 | void BrownianMotionPose::propagate(const Eigen::Ref& cur_state, Eigen::Ref prop_state) 121 | { 122 | prop_state = cur_state; 123 | } 124 | 125 | 126 | void BrownianMotionPose::motion(const Eigen::Ref& cur_state, Eigen::Ref mot_state) 127 | { 128 | propagate(cur_state, mot_state); 129 | 130 | MatrixXf sample(7, mot_state.cols()); 131 | sample = getNoiseSample(mot_state.cols()); 132 | 133 | mot_state.topRows<3>() += sample.topRows<3>(); 134 | addAxisangleDisturbance(sample.bottomRows<4>(), mot_state.bottomRows<4>()); 135 | } 136 | 137 | 138 | Eigen::MatrixXf BrownianMotionPose::getNoiseSample(const int num) 139 | { 140 | MatrixXf sample(7, num); 141 | 142 | /* Position */ 143 | for (unsigned int i = 0; i < num; ++i) 144 | { 145 | sample(0, i) = gaussian_random_pos_xy_(); 146 | sample(1, i) = gaussian_random_pos_xy_(); 147 | sample(2, i) = gaussian_random_pos_z_(); 148 | } 149 | 150 | /* Axis-angle vector */ 151 | /* Generate points on the spherical cap around the north pole [1]. */ 152 | /* [1] http://math.stackexchange.com/a/205589/81266 */ 153 | for(unsigned int i = 0; i < num; ++i) 154 | { 155 | float z = gaussian_random_cone_() * (1 - cos(cone_angle_)) + cos(cone_angle_); 156 | float phi = gaussian_random_cone_() * (2.0 * M_PI); 157 | float x = sqrt(1 - (z * z)) * cos(phi); 158 | float y = sqrt(1 - (z * z)) * sin(phi); 159 | 160 | sample(3, i) = x; 161 | sample(4, i) = y; 162 | sample(5, i) = z; 163 | sample(6, i) = gaussian_random_theta_(); 164 | } 165 | 166 | return sample; 167 | } 168 | 169 | 170 | void BrownianMotionPose::addAxisangleDisturbance(const Ref& disturbance_vec, Ref current_vec) 171 | { 172 | for (unsigned int i = 0; i < current_vec.cols(); ++i) 173 | { 174 | float ang = current_vec(3, i) + disturbance_vec(3, i); 175 | 176 | if (ang > M_PI) ang -= 2.0 * M_PI; 177 | else if (ang <= -M_PI) ang += 2.0 * M_PI; 178 | 179 | 180 | /* Find the rotation axis 'u' and rotation angle 'rot' [1] */ 181 | Vector3f def_dir(0.0, 0.0, 1.0); 182 | 183 | Vector3f u = def_dir.cross(current_vec.col(i).head<3>()).normalized(); 184 | 185 | float rot = static_cast(std::acos(current_vec.col(i).head<3>().dot(def_dir))); 186 | 187 | 188 | /* Convert rotation axis and angle to 3x3 rotation matrix [2] */ 189 | /* [2] https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle */ 190 | Matrix3f cross_matrix; 191 | cross_matrix << 0, -u(2), u(1), 192 | u(2), 0, -u(0), 193 | -u(1), u(0), 0; 194 | 195 | Matrix3f R = std::cos(rot) * Matrix3f::Identity() + std::sin(rot) * cross_matrix + (1 - std::cos(rot)) * (u * u.transpose()); 196 | 197 | 198 | current_vec.col(i).head<3>() = (R * disturbance_vec.col(i).head<3>()).normalized(); 199 | current_vec(3, i) = ang; 200 | } 201 | } 202 | -------------------------------------------------------------------------------- /src/hand-tracking/src/DrawFwdKinPoses.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | 12 | using namespace bfl; 13 | using namespace Eigen; 14 | 15 | 16 | DrawFwdKinPoses::DrawFwdKinPoses() noexcept { } 17 | 18 | 19 | DrawFwdKinPoses::~DrawFwdKinPoses() noexcept { } 20 | 21 | 22 | DrawFwdKinPoses::DrawFwdKinPoses(DrawFwdKinPoses&& pf_prediction) noexcept : 23 | state_model_(std::move(pf_prediction.state_model_)), 24 | exogenous_model_(std::move(pf_prediction.exogenous_model_)) { }; 25 | 26 | 27 | DrawFwdKinPoses& DrawFwdKinPoses::operator=(DrawFwdKinPoses&& pf_prediction) noexcept 28 | { 29 | state_model_ = std::move(pf_prediction.state_model_); 30 | 31 | exogenous_model_ = std::move(pf_prediction.exogenous_model_); 32 | 33 | return *this; 34 | } 35 | 36 | 37 | StateModel& DrawFwdKinPoses::getStateModel() 38 | { 39 | return *state_model_; 40 | } 41 | 42 | 43 | void DrawFwdKinPoses::setStateModel(std::unique_ptr state_model) 44 | { 45 | state_model_ = std::move(state_model); 46 | } 47 | 48 | 49 | ExogenousModel& DrawFwdKinPoses::getExogenousModel() 50 | { 51 | return *exogenous_model_; 52 | } 53 | 54 | 55 | void DrawFwdKinPoses::setExogenousModel(std::unique_ptr exogenous_model) 56 | { 57 | exogenous_model_ = std::move(exogenous_model); 58 | } 59 | 60 | 61 | void DrawFwdKinPoses::predictStep(const Ref& prev_states, const Ref& prev_weights, 62 | Ref pred_states, Ref pred_weights) 63 | { 64 | VectorXf sorted_cor = prev_weights; 65 | std::sort(sorted_cor.data(), sorted_cor.data() + sorted_cor.size()); 66 | float threshold = sorted_cor.tail(6)(0); 67 | 68 | exogenous_model_->setProperty("ICFW_DELTA"); 69 | 70 | for (int j = 0; j < prev_weights.rows(); ++j) 71 | { 72 | VectorXf tmp_states = VectorXf::Zero(prev_states.rows()); 73 | 74 | if (!getSkipExogenous()) 75 | exogenous_model_->propagate(prev_states.col(j), tmp_states); 76 | else 77 | tmp_states = prev_states.col(j); 78 | 79 | if (!getSkipState() && prev_weights(j) <= threshold) 80 | state_model_->motion(tmp_states, pred_states.col(j)); 81 | else 82 | pred_states.col(j) = tmp_states; 83 | } 84 | 85 | pred_weights = prev_weights; 86 | } 87 | -------------------------------------------------------------------------------- /src/hand-tracking/src/FwdKinModel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | using namespace bfl; 21 | using namespace Eigen; 22 | using namespace iCub::ctrl; 23 | using namespace iCub::iKin; 24 | using namespace yarp::math; 25 | using namespace yarp::os; 26 | using namespace yarp::sig; 27 | 28 | 29 | FwdKinModel::FwdKinModel() noexcept { } 30 | 31 | 32 | FwdKinModel::~FwdKinModel() noexcept { } 33 | 34 | 35 | void FwdKinModel::propagate(const Ref& cur_state, Ref prop_state) 36 | { 37 | prop_state.topRows<3>() = cur_state.topRows<3>().colwise() + delta_hand_pose_.head<3>().cast(); 38 | 39 | prop_state.middleRows<3>(3) = (cur_state.middleRows<3>(3).colwise() + delta_hand_pose_.middleRows<3>(3).cast()); 40 | prop_state.middleRows<3>(3).colwise().normalize(); 41 | 42 | RowVectorXf ang = cur_state.bottomRows<1>().array() + static_cast(delta_angle_); 43 | 44 | for (unsigned int i = 0; i < ang.cols(); ++i) 45 | { 46 | if (ang(i) > M_PI) ang(i) -= 2.0 * M_PI; 47 | else if (ang(i) <= -M_PI) ang(i) += 2.0 * M_PI; 48 | } 49 | 50 | prop_state.bottomRows<1>() = ang; 51 | } 52 | 53 | 54 | MatrixXf FwdKinModel::getExogenousMatrix() 55 | { 56 | std::cerr << "ERROR::PFPREDICTION::SETEXOGENOUSMODEL" << std::endl; 57 | std::cerr << "ERROR:\n\tCall to unimplemented base class method."; 58 | 59 | return MatrixXf::Zero(1, 1); 60 | } 61 | 62 | 63 | bool FwdKinModel::setProperty(const std::string& property) 64 | { 65 | if (property == "ICFW_DELTA") 66 | return setDeltaMotion(); 67 | 68 | if (property == "ICFW_INIT") 69 | { 70 | initialize_delta_ = true; 71 | return setDeltaMotion(); 72 | } 73 | 74 | return false; 75 | } 76 | 77 | 78 | bool FwdKinModel::setDeltaMotion() 79 | { 80 | VectorXd ee_pose = readPose(); 81 | 82 | if (!initialize_delta_) 83 | { 84 | delta_hand_pose_.head<3>() = ee_pose.head<3>() - prev_ee_pose_.head<3>(); 85 | 86 | delta_hand_pose_.middleRows<3>(3) = ee_pose.middleRows<3>(3) - prev_ee_pose_.middleRows<3>(3); 87 | 88 | delta_angle_ = ee_pose(6) - prev_ee_pose_(6); 89 | if (delta_angle_ > M_PI) delta_angle_ -= 2.0 * M_PI; 90 | else if (delta_angle_ <= -M_PI) delta_angle_ += 2.0 * M_PI; 91 | } 92 | else 93 | { 94 | delta_hand_pose_ = VectorXd::Zero(6); 95 | delta_angle_ = 0.0; 96 | initialize_delta_ = false; 97 | } 98 | 99 | prev_ee_pose_ = ee_pose; 100 | 101 | return true; 102 | } 103 | -------------------------------------------------------------------------------- /src/hand-tracking/src/GatePose.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | 12 | using namespace bfl; 13 | using namespace cv; 14 | using namespace Eigen; 15 | 16 | 17 | GatePose::GatePose(std::unique_ptr visual_correction, 18 | const double gate_x, const double gate_y, const double gate_z, 19 | const double gate_rotation, 20 | const double gate_aperture) noexcept : 21 | PFVisualCorrectionDecorator(std::move(visual_correction)), 22 | gate_x_(gate_x), gate_y_(gate_y), gate_z_(gate_z), 23 | gate_aperture_((M_PI / 180.0) * gate_aperture), 24 | gate_rotation_((M_PI / 180.0) * gate_rotation) 25 | { 26 | ee_pose_ = VectorXd::Zero(7); 27 | } 28 | 29 | 30 | GatePose::GatePose(std::unique_ptr visual_correction) noexcept : 31 | GatePose(std::move(visual_correction), 0.1, 0.1, 0.1, 5, 30) { } 32 | 33 | 34 | GatePose::~GatePose() noexcept { } 35 | 36 | 37 | void GatePose::innovation(const Ref& pred_states, cv::InputArray measurements, Ref innovations) 38 | { 39 | PFVisualCorrectionDecorator::innovation(pred_states, measurements, innovations); 40 | } 41 | 42 | 43 | double GatePose::likelihood(const Ref& innovations) 44 | { 45 | return PFVisualCorrectionDecorator::likelihood(innovations); 46 | } 47 | 48 | 49 | VisualObservationModel& GatePose::getVisualObservationModel() 50 | { 51 | return PFVisualCorrectionDecorator::getVisualObservationModel(); 52 | } 53 | 54 | 55 | void GatePose::setVisualObservationModel(std::unique_ptr visual_observation_model) 56 | { 57 | PFVisualCorrectionDecorator::setVisualObservationModel(std::move(visual_observation_model)); 58 | } 59 | 60 | 61 | void GatePose::correctStep(const Ref& pred_states, const Ref& pred_weights, cv::InputArray measurements, 62 | Ref cor_states, Ref cor_weights) 63 | { 64 | PFVisualCorrectionDecorator::correctStep(pred_states, pred_weights, measurements, 65 | cor_states, cor_weights); 66 | 67 | ee_pose_ = readPose(); 68 | 69 | for (int i = 0; i < cor_states.cols(); ++i) 70 | { 71 | if (!isInsideEllipsoid(cor_states.col(i).head<3>()) || 72 | !isInsideCone (cor_states.col(i).middleRows<3>(3)) || 73 | !isWithinRotation (cor_states(6, i)) ) 74 | cor_weights(i) = std::numeric_limits::min(); 75 | } 76 | } 77 | 78 | 79 | bool GatePose::isInsideEllipsoid(const Ref& state) 80 | { 81 | return ( (abs(state(0) - ee_pose_(0)) <= gate_x_) && 82 | (abs(state(1) - ee_pose_(1)) <= gate_y_) && 83 | (abs(state(2) - ee_pose_(2)) <= gate_z_) ); 84 | } 85 | 86 | 87 | bool GatePose::isWithinRotation(float rot_angle) 88 | { 89 | float ang_diff = abs(rot_angle - ee_pose_(6)); 90 | 91 | return (ang_diff <= gate_rotation_); 92 | } 93 | 94 | 95 | bool GatePose::isInsideCone(const Ref& state) 96 | { 97 | /* See: http://stackoverflow.com/questions/10768142/verify-if-point-is-inside-a-cone-in-3d-space#10772759 */ 98 | 99 | double half_aperture = gate_aperture_ / 2.0; 100 | 101 | VectorXd test_direction = -state.cast(); 102 | 103 | VectorXd fwdkin_direction = -ee_pose_.middleRows<3>(3); 104 | 105 | return ( (test_direction.dot(fwdkin_direction) / test_direction.norm() / fwdkin_direction.norm()) >= cos(half_aperture) ); 106 | } 107 | -------------------------------------------------------------------------------- /src/hand-tracking/src/InitiCubArm.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | 12 | using namespace Eigen; 13 | using namespace iCub::iKin; 14 | using namespace iCub::ctrl; 15 | using namespace yarp::math; 16 | using namespace yarp::sig; 17 | using namespace yarp::os; 18 | 19 | 20 | InitiCubArm::InitiCubArm(const ConstString& port_prefix, const ConstString& cam_sel, const ConstString& laterality) noexcept : 21 | icub_kin_arm_(iCubArm(laterality + "_v2")), icub_kin_finger_{iCubFinger(laterality + "_thumb"), iCubFinger(laterality + "_index"), iCubFinger(laterality + "_middle")} 22 | { 23 | icub_kin_arm_.setAllConstraints(false); 24 | icub_kin_arm_.releaseLink(0); 25 | icub_kin_arm_.releaseLink(1); 26 | icub_kin_arm_.releaseLink(2); 27 | 28 | icub_kin_finger_[0].setAllConstraints(false); 29 | icub_kin_finger_[1].setAllConstraints(false); 30 | icub_kin_finger_[2].setAllConstraints(false); 31 | 32 | port_arm_enc_.open ("/" + port_prefix + "/cam/" + cam_sel + "/" + laterality + "_arm:i"); 33 | port_torso_enc_.open("/" + port_prefix + "/cam/" + cam_sel + "/torso:i"); 34 | } 35 | 36 | 37 | InitiCubArm::InitiCubArm(const ConstString& cam_sel, const ConstString& laterality) noexcept : 38 | InitiCubArm("InitiCubArm", cam_sel, laterality) { } 39 | 40 | 41 | InitiCubArm::~InitiCubArm() noexcept 42 | { 43 | port_arm_enc_.close(); 44 | port_torso_enc_.close(); 45 | } 46 | 47 | 48 | void InitiCubArm::initialize(Eigen::Ref state, Eigen::Ref weight) 49 | { 50 | Vector ee_pose = icub_kin_arm_.EndEffPose(CTRL_DEG2RAD * readRootToEE()); 51 | 52 | Map init_hand_pose(ee_pose.data(), 7, 1); 53 | 54 | for (int i = 0; i < state.cols(); ++i) 55 | state.col(i) = init_hand_pose.cast(); 56 | 57 | weight.fill(1.0 / state.cols()); 58 | } 59 | 60 | 61 | Vector InitiCubArm::readTorso() 62 | { 63 | Bottle* b = port_torso_enc_.read(true); 64 | if (!b) return Vector(3, 0.0); 65 | 66 | Vector torso_enc(3); 67 | torso_enc(0) = b->get(2).asDouble(); 68 | torso_enc(1) = b->get(1).asDouble(); 69 | torso_enc(2) = b->get(0).asDouble(); 70 | 71 | return torso_enc; 72 | } 73 | 74 | 75 | Vector InitiCubArm::readRootToEE() 76 | { 77 | Bottle* b = port_arm_enc_.read(true); 78 | if (!b) return Vector(10, 0.0); 79 | 80 | Vector root_ee_enc(10); 81 | 82 | root_ee_enc.setSubvector(0, readTorso()); 83 | 84 | for (size_t i = 0; i < 7; ++i) 85 | root_ee_enc(i + 3) = b->get(i).asDouble(); 86 | 87 | return root_ee_enc; 88 | } 89 | -------------------------------------------------------------------------------- /src/hand-tracking/src/PlayFwdKinModel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | using namespace bfl; 21 | using namespace Eigen; 22 | using namespace iCub::ctrl; 23 | using namespace iCub::iKin; 24 | using namespace yarp::eigen; 25 | using namespace yarp::math; 26 | using namespace yarp::os; 27 | using namespace yarp::sig; 28 | 29 | 30 | PlayFwdKinModel::PlayFwdKinModel(const ConstString& robot, const ConstString& laterality, const ConstString& port_prefix) noexcept : 31 | icub_kin_arm_(iCubArm(laterality+"_v2")), 32 | robot_(robot), laterality_(laterality), port_prefix_(port_prefix) 33 | { 34 | port_arm_enc_.open ("/hand-tracking/" + ID_ + "/" + port_prefix_ + "/" + laterality_ + "_arm:i"); 35 | port_torso_enc_.open("/hand-tracking/" + ID_ + "/" + port_prefix_ + "/torso:i"); 36 | 37 | icub_kin_arm_.setAllConstraints(false); 38 | icub_kin_arm_.releaseLink(0); 39 | icub_kin_arm_.releaseLink(1); 40 | icub_kin_arm_.releaseLink(2); 41 | 42 | yInfo() << log_ID_ << "Succesfully initialized."; 43 | } 44 | 45 | PlayFwdKinModel::~PlayFwdKinModel() noexcept 46 | { 47 | port_torso_enc_.interrupt(); 48 | port_torso_enc_.close(); 49 | 50 | port_arm_enc_.interrupt(); 51 | port_arm_enc_.close(); 52 | } 53 | 54 | 55 | bool PlayFwdKinModel::setProperty(const std::string& property) 56 | { 57 | return FwdKinModel::setProperty(property); 58 | } 59 | 60 | 61 | VectorXd PlayFwdKinModel::readPose() 62 | { 63 | Vector ee_pose = icub_kin_arm_.EndEffPose(CTRL_DEG2RAD * readRootToEE()); 64 | return toEigen(ee_pose); 65 | } 66 | 67 | 68 | Vector PlayFwdKinModel::readTorso() 69 | { 70 | Bottle* b = port_torso_enc_.read(true); 71 | if (!b) return Vector(3, 0.0); 72 | 73 | Vector torso_enc(3); 74 | torso_enc(0) = b->get(2).asDouble(); 75 | torso_enc(1) = b->get(1).asDouble(); 76 | torso_enc(2) = b->get(0).asDouble(); 77 | 78 | return torso_enc; 79 | } 80 | 81 | 82 | Vector PlayFwdKinModel::readRootToEE() 83 | { 84 | Bottle* b = port_arm_enc_.read(true); 85 | if (!b) return Vector(10, 0.0); 86 | 87 | Vector root_ee_enc(10); 88 | root_ee_enc.setSubvector(0, readTorso()); 89 | for (size_t i = 0; i < 7; ++i) 90 | root_ee_enc(i + 3) = b->get(i).asDouble(); 91 | 92 | return root_ee_enc; 93 | } 94 | -------------------------------------------------------------------------------- /src/hand-tracking/src/PlayGatePose.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | using namespace bfl; 16 | using namespace cv; 17 | using namespace Eigen; 18 | using namespace iCub::ctrl; 19 | using namespace iCub::iKin; 20 | using namespace yarp::eigen; 21 | using namespace yarp::math; 22 | using namespace yarp::os; 23 | using namespace yarp::sig; 24 | 25 | 26 | PlayGatePose::PlayGatePose(std::unique_ptr visual_correction, 27 | const double gate_x, const double gate_y, const double gate_z, 28 | const double gate_rotation, 29 | const double gate_aperture, 30 | const yarp::os::ConstString& robot, const yarp::os::ConstString& laterality, const yarp::os::ConstString& port_prefix) noexcept : 31 | GatePose(std::move(visual_correction), 32 | gate_x, gate_y, gate_z, 33 | gate_rotation, 34 | gate_aperture), 35 | icub_kin_arm_(iCubArm(laterality + "_v2")), robot_(robot), laterality_(laterality), port_prefix_(port_prefix) 36 | { 37 | icub_kin_arm_.setAllConstraints(false); 38 | icub_kin_arm_.releaseLink(0); 39 | icub_kin_arm_.releaseLink(1); 40 | icub_kin_arm_.releaseLink(2); 41 | 42 | 43 | port_arm_enc_.open ("/hand-tracking/" + ID_ + "/" + port_prefix_ + "/" + laterality_ + "_arm:i"); 44 | port_torso_enc_.open("/hand-tracking/" + ID_ + "/" + port_prefix_ + "/torso:i"); 45 | 46 | 47 | yInfo() << log_ID_ << "Succesfully initialized."; 48 | } 49 | 50 | 51 | PlayGatePose::PlayGatePose(std::unique_ptr visual_correction, 52 | const yarp::os::ConstString& robot, const yarp::os::ConstString& laterality, const yarp::os::ConstString& port_prefix) noexcept : 53 | PlayGatePose(std::move(visual_correction), 0.1, 0.1, 0.1, 5, 30, robot, laterality, port_prefix) { } 54 | 55 | 56 | PlayGatePose::~PlayGatePose() noexcept { } 57 | 58 | 59 | Vector PlayGatePose::readTorso() 60 | { 61 | Bottle* b = port_torso_enc_.read(true); 62 | if (!b) return Vector(3, 0.0); 63 | 64 | Vector torso_enc(3); 65 | torso_enc(0) = b->get(2).asDouble(); 66 | torso_enc(1) = b->get(1).asDouble(); 67 | torso_enc(2) = b->get(0).asDouble(); 68 | 69 | return torso_enc; 70 | } 71 | 72 | 73 | Vector PlayGatePose::readRootToEE() 74 | { 75 | Bottle* b = port_arm_enc_.read(true); 76 | if (!b) return Vector(10, 0.0); 77 | 78 | Vector root_ee_enc(10); 79 | root_ee_enc.setSubvector(0, readTorso()); 80 | for (size_t i = 0; i < 7; ++i) 81 | { 82 | root_ee_enc(i+3) = b->get(i).asDouble(); 83 | } 84 | 85 | return root_ee_enc; 86 | } 87 | 88 | 89 | VectorXd PlayGatePose::readPose() 90 | { 91 | Vector ee_pose = icub_kin_arm_.EndEffPose(CTRL_DEG2RAD * readRootToEE()); 92 | 93 | return toEigen(ee_pose); 94 | } 95 | -------------------------------------------------------------------------------- /src/hand-tracking/src/VisualSIS.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | using namespace bfl; 27 | using namespace cv; 28 | using namespace Eigen; 29 | using namespace iCub::ctrl; 30 | using namespace iCub::iKin; 31 | using namespace yarp::eigen; 32 | using namespace yarp::math; 33 | using namespace yarp::os; 34 | using yarp::sig::Vector; 35 | using yarp::sig::ImageOf; 36 | using yarp::sig::PixelRgb; 37 | 38 | 39 | VisualSIS::VisualSIS(const yarp::os::ConstString& cam_sel, 40 | const int img_width, const int img_height, 41 | const int num_particles, 42 | const double resample_ratio) : 43 | cam_sel_(cam_sel), 44 | img_width_(img_width), 45 | img_height_(img_height), 46 | num_particles_(num_particles), 47 | resample_ratio_(resample_ratio) 48 | { 49 | descriptor_length_ = (img_width_/block_size_*2-1) * (img_height_/block_size_*2-1) * bin_number_ * 4; 50 | 51 | 52 | cuda_hog_ = cuda::HOG::create(Size(img_width_, img_height_), Size(block_size_, block_size_), Size(block_size_/2, block_size_/2), Size(block_size_/2, block_size_/2), bin_number_); 53 | cuda_hog_->setDescriptorFormat(cuda::HOG::DESCR_FORMAT_ROW_BY_ROW); 54 | cuda_hog_->setGammaCorrection(true); 55 | cuda_hog_->setWinStride(Size(img_width_, img_height_)); 56 | 57 | 58 | port_image_in_.open ("/hand-tracking/" + cam_sel_ + "/img:i"); 59 | port_estimates_out_.open("/hand-tracking/" + cam_sel_ + "/result/estimates:o"); 60 | 61 | img_in_.resize(320, 240); 62 | img_in_.zero(); 63 | 64 | setCommandPort(); 65 | } 66 | 67 | 68 | void VisualSIS::initialization() 69 | { 70 | pred_particle_ = MatrixXf(7, num_particles_); 71 | pred_weight_ = VectorXf(num_particles_, 1); 72 | 73 | cor_particle_ = MatrixXf(7, num_particles_); 74 | cor_weight_ = VectorXf(num_particles_, 1); 75 | 76 | estimate_extraction_.clear(); 77 | 78 | initialization_->initialize(pred_particle_, pred_weight_); 79 | 80 | prediction_->getExogenousModel().setProperty("ICFW_INIT"); 81 | 82 | skip("all", false); 83 | } 84 | 85 | 86 | VisualSIS::~VisualSIS() noexcept 87 | { 88 | port_image_in_.close(); 89 | port_estimates_out_.close(); 90 | } 91 | 92 | 93 | void VisualSIS::filteringStep() 94 | { 95 | std::vector descriptors_cam_left (descriptor_length_); 96 | cuda::GpuMat cuda_img (Size(img_width_, img_height_), CV_8UC3); 97 | cuda::GpuMat cuda_img_alpha (Size(img_width_, img_height_), CV_8UC4); 98 | cuda::GpuMat descriptors_cam_cuda (Size(descriptor_length_, 1), CV_32F ); 99 | 100 | 101 | ImageOf* tmp_imgin = YARP_NULLPTR; 102 | tmp_imgin = port_image_in_.read(false); 103 | if (tmp_imgin != YARP_NULLPTR) 104 | { 105 | init_img_in_ = true; 106 | img_in_ = *tmp_imgin; 107 | } 108 | 109 | if (init_img_in_) 110 | { 111 | /* PROCESS CURRENT MEASUREMENT */ 112 | Mat measurement; 113 | 114 | measurement = cvarrToMat(img_in_.getIplImage()); 115 | cuda_img.upload(measurement); 116 | cuda::resize(cuda_img, cuda_img, Size(img_width_, img_height_)); 117 | cuda::cvtColor(cuda_img, cuda_img_alpha, COLOR_BGR2BGRA, 4); 118 | cuda_hog_->compute(cuda_img_alpha, descriptors_cam_cuda); 119 | descriptors_cam_cuda.download(descriptors_cam_left); 120 | 121 | /* PREDICTION */ 122 | if (getFilteringStep() != 0) 123 | prediction_->predict(cor_particle_, cor_weight_, 124 | pred_particle_, pred_weight_); 125 | 126 | /* CORRECTION */ 127 | correction_->getVisualObservationModel().setProperty("VP_PARAMS"); 128 | correction_->correct(pred_particle_, pred_weight_, descriptors_cam_left, 129 | cor_particle_, cor_weight_); 130 | cor_weight_ /= cor_weight_.sum(); 131 | 132 | 133 | /* STATE ESTIMATE EXTRACTION FROM PARTICLE SET */ 134 | VectorXf out_particle = estimate_extraction_.extract(cor_particle_, cor_weight_); 135 | 136 | 137 | /* RESAMPLING */ 138 | std::cout << "Step: " << getFilteringStep() << std::endl; 139 | std::cout << "Neff: " << resampling_->neff(cor_weight_) << std::endl; 140 | if (resampling_->neff(cor_weight_) < std::round(num_particles_ * resample_ratio_)) 141 | { 142 | std::cout << "Resampling!" << std::endl; 143 | 144 | MatrixXf res_particle(7, num_particles_); 145 | VectorXf res_weight(num_particles_, 1); 146 | VectorXf res_parent(num_particles_, 1); 147 | 148 | resampling_->resample(cor_particle_, cor_weight_, 149 | res_particle, res_weight, 150 | res_parent); 151 | 152 | cor_particle_ = res_particle; 153 | cor_weight_ = res_weight; 154 | } 155 | 156 | /* STATE ESTIMATE OUTPUT */ 157 | /* INDEX FINGERTIP */ 158 | // Vector q = readRootToEE(); 159 | // icub_kin_arm_.setAng(q.subVector(0, 9) * (M_PI/180.0)); 160 | // Vector chainjoints; 161 | // if (analogs_) icub_kin_finger_[1].getChainJoints(q.subVector(3, 18), analogs, chainjoints, right_hand_analogs_bounds_); 162 | // else icub_kin_finger_[1].getChainJoints(q.subVector(3, 18), chainjoints); 163 | // icub_kin_finger_[1].setAng(chainjoints * (M_PI/180.0)); 164 | // 165 | // Vector l_ee_t(3); 166 | // toEigen(l_ee_t) = out_particle.col(0).head(3).cast(); 167 | // l_ee_t.push_back(1.0); 168 | // 169 | // Vector l_ee_o(3); 170 | // toEigen(l_ee_o) = out_particle.col(0).tail(3).normalized().cast(); 171 | // l_ee_o.push_back(static_cast(out_particle.col(0).tail(3).norm())); 172 | // 173 | // yarp::sig::Matrix l_Ha = axis2dcm(l_ee_o); 174 | // l_Ha.setCol(3, l_ee_t); 175 | // Vector l_i_x = (l_Ha * (icub_kin_finger_[1].getH(3, true).getCol(3))).subVector(0, 2); 176 | // Vector l_i_o = dcm2axis(l_Ha * icub_kin_finger_[1].getH(3, true)); 177 | // l_i_o.setSubvector(0, l_i_o.subVector(0, 2) * l_i_o[3]); 178 | // 179 | // 180 | // Vector r_ee_t(3); 181 | // toEigen(r_ee_t) = out_particle.col(1).head(3).cast(); 182 | // r_ee_t.push_back(1.0); 183 | // 184 | // Vector r_ee_o(3); 185 | // toEigen(r_ee_o) = out_particle.col(1).tail(3).normalized().cast(); 186 | // r_ee_o.push_back(static_cast(out_particle.col(1).tail(3).norm())); 187 | // 188 | // yarp::sig::Matrix r_Ha = axis2dcm(r_ee_o); 189 | // r_Ha.setCol(3, r_ee_t); 190 | // Vector r_i_x = (r_Ha * (icub_kin_finger_[1].getH(3, true).getCol(3))).subVector(0, 2); 191 | // Vector r_i_o = dcm2axis(r_Ha * icub_kin_finger_[1].getH(3, true)); 192 | // r_i_o.setSubvector(0, r_i_o.subVector(0, 2) * r_i_o[3]); 193 | // 194 | // 195 | // Vector& estimates_out = port_estimates_out_.prepare(); 196 | // estimates_out.resize(12); 197 | // estimates_out.setSubvector(0, l_i_x); 198 | // estimates_out.setSubvector(3, l_i_o.subVector(0, 2)); 199 | // estimates_out.setSubvector(6, r_i_x); 200 | // estimates_out.setSubvector(9, r_i_o.subVector(0, 2)); 201 | // port_estimates_out_.write(); 202 | 203 | /* PALM */ 204 | Vector& estimates_out = port_estimates_out_.prepare(); 205 | estimates_out.resize(7); 206 | toEigen(estimates_out) = out_particle.cast(); 207 | port_estimates_out_.write(); 208 | /* ********** */ 209 | } 210 | } 211 | 212 | 213 | void VisualSIS::getResult() { } 214 | 215 | 216 | bool VisualSIS::attach(yarp::os::Port &source) 217 | { 218 | return this->yarp().attachAsServer(source); 219 | } 220 | 221 | 222 | bool VisualSIS::setCommandPort() 223 | { 224 | std::cout << "Opening RPC command port." << std::endl; 225 | if (!port_rpc_command_.open("/hand-tracking/" + cam_sel_ + "/cmd:i")) 226 | { 227 | std::cerr << "Cannot open the RPC command port." << std::endl; 228 | return false; 229 | } 230 | if (!attach(port_rpc_command_)) 231 | { 232 | std::cerr << "Cannot attach the RPC command port." << std::endl; 233 | return false; 234 | } 235 | std::cout << "RPC command port opened and attached. Ready to recieve commands!" << std::endl; 236 | 237 | return true; 238 | } 239 | 240 | 241 | bool VisualSIS::run_filter() 242 | { 243 | run(); 244 | 245 | return true; 246 | } 247 | 248 | 249 | bool VisualSIS::reset_filter() 250 | { 251 | reset(); 252 | 253 | return true; 254 | } 255 | 256 | 257 | bool VisualSIS::stop_filter() 258 | { 259 | reboot(); 260 | 261 | return true; 262 | } 263 | 264 | 265 | bool VisualSIS::skip_step(const std::string& what_step, const bool status) 266 | { 267 | return skip(what_step, status); 268 | } 269 | 270 | 271 | bool VisualSIS::use_analogs(const bool status) 272 | { 273 | if (status) 274 | return correction_->getVisualObservationModel().setProperty("VP_ANALOGS_ON"); 275 | else 276 | return correction_->getVisualObservationModel().setProperty("VP_ANALOGS_OFF"); 277 | } 278 | 279 | 280 | std::vector VisualSIS::get_info() 281 | { 282 | std::vector info; 283 | 284 | info.push_back("<| Information about Visual SIR Particle Filter |>"); 285 | info.push_back("<| The Particle Filter is " + std::string(isRunning() ? "not " : "") + "running |>"); 286 | info.push_back("<| Filtering step: " + std::to_string(getFilteringStep()) + " |>"); 287 | info.push_back("<| Using " + cam_sel_ + " camera images |>"); 288 | info.push_back("<| Using " + std::to_string(num_particles_) + " particles |>"); 289 | 290 | std::vector est_ext_info = estimate_extraction_.getInfo(); 291 | 292 | info.insert(info.end(), est_ext_info.begin(), est_ext_info.end()); 293 | 294 | return info; 295 | } 296 | 297 | 298 | bool VisualSIS::set_estimates_extraction_method(const std::string& method) 299 | { 300 | if (method == "mean") 301 | { 302 | estimate_extraction_.setMethod(EstimatesExtraction::ExtractionMethod::mean); 303 | 304 | return true; 305 | } 306 | else if (method == "smean") 307 | { 308 | estimate_extraction_.setMethod(EstimatesExtraction::ExtractionMethod::smean); 309 | 310 | return true; 311 | } 312 | else if (method == "wmean") 313 | { 314 | estimate_extraction_.setMethod(EstimatesExtraction::ExtractionMethod::wmean); 315 | 316 | return true; 317 | } 318 | else if (method == "emean") 319 | { 320 | estimate_extraction_.setMethod(EstimatesExtraction::ExtractionMethod::emean); 321 | 322 | return true; 323 | } 324 | else if (method == "mode") 325 | { 326 | estimate_extraction_.setMethod(EstimatesExtraction::ExtractionMethod::mode); 327 | 328 | return true; 329 | } 330 | else if (method == "smode") 331 | { 332 | estimate_extraction_.setMethod(EstimatesExtraction::ExtractionMethod::smode); 333 | 334 | return true; 335 | } 336 | else if (method == "wmode") 337 | { 338 | estimate_extraction_.setMethod(EstimatesExtraction::ExtractionMethod::wmode); 339 | 340 | return true; 341 | } 342 | else if (method == "emode") 343 | { 344 | estimate_extraction_.setMethod(EstimatesExtraction::ExtractionMethod::emode); 345 | 346 | return true; 347 | } 348 | 349 | return false; 350 | } 351 | 352 | 353 | bool VisualSIS::set_mobile_average_window(const int16_t window) 354 | { 355 | if (window > 0) 356 | return estimate_extraction_.setMobileAverageWindowSize(window); 357 | else 358 | return false; 359 | } 360 | 361 | 362 | bool VisualSIS::quit() 363 | { 364 | return teardown(); 365 | } 366 | -------------------------------------------------------------------------------- /src/hand-tracking/src/VisualUpdateParticles.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | using namespace bfl; 24 | using namespace cv; 25 | using namespace Eigen; 26 | 27 | 28 | VisualUpdateParticles::VisualUpdateParticles(std::unique_ptr observation_model) noexcept : 29 | VisualUpdateParticles(std::move(observation_model), 0.001, 1) { } 30 | 31 | 32 | VisualUpdateParticles::VisualUpdateParticles(std::unique_ptr observation_model, const double likelihood_gain) noexcept : 33 | VisualUpdateParticles(std::move(observation_model), likelihood_gain, 1) { } 34 | 35 | 36 | VisualUpdateParticles::VisualUpdateParticles(std::unique_ptr observation_model, const double likelihood_gain, const int num_cuda_stream) noexcept : 37 | observation_model_(std::move(observation_model)), 38 | likelihood_gain_(likelihood_gain), 39 | num_cuda_stream_(num_cuda_stream), 40 | num_img_stream_(observation_model_->getOGLTilesNumber()), 41 | cuda_stream_(std::vector(num_cuda_stream)) 42 | { 43 | int block_size = 16; 44 | int bin_number = 9; 45 | unsigned int img_width = observation_model_->getCamWidth(); 46 | unsigned int img_height = observation_model_->getCamHeight(); 47 | unsigned int ogl_tiles_cols = observation_model_->getOGLTilesCols(); 48 | unsigned int ogl_tiles_rows = observation_model_->getOGLTilesRows(); 49 | unsigned int feature_dim = (img_width/block_size*2-1) * (img_height/block_size*2-1) * bin_number * 4; 50 | 51 | cuda_hog_ = cuda::HOG::create(Size(img_width, img_height), Size(block_size, block_size), Size(block_size/2, block_size/2), Size(block_size/2, block_size/2), bin_number); 52 | cuda_hog_->setDescriptorFormat(cuda::HOG::DESCR_FORMAT_ROW_BY_ROW); 53 | cuda_hog_->setGammaCorrection(true); 54 | cuda_hog_->setWinStride(Size(img_width, img_height)); 55 | 56 | for (int s = 0; s < num_cuda_stream_; ++s) 57 | { 58 | hand_rendered_.emplace_back (Mat( Size(img_width * ogl_tiles_cols, img_height* ogl_tiles_rows), CV_8UC3)); 59 | cuda_img_.emplace_back (cuda::GpuMat(Size(img_width * ogl_tiles_cols, img_height* ogl_tiles_rows), CV_8UC3)); 60 | cuda_img_alpha_.emplace_back (cuda::GpuMat(Size(img_width * ogl_tiles_cols, img_height* ogl_tiles_rows), CV_8UC4)); 61 | cuda_descriptors_.emplace_back(cuda::GpuMat(Size(num_img_stream_, feature_dim), CV_32F )); 62 | cpu_descriptors_.emplace_back (Mat( Size(num_img_stream_, feature_dim), CV_32F )); 63 | } 64 | } 65 | 66 | 67 | VisualUpdateParticles::~VisualUpdateParticles() noexcept { } 68 | 69 | 70 | void VisualUpdateParticles::innovation(const Ref& pred_states, cv::InputArray measurements, Ref innovations) 71 | { 72 | for (int s = 0; s < num_cuda_stream_; ++s) 73 | observation_model_->observe(pred_states.block(0, s * num_img_stream_, 7, num_img_stream_), hand_rendered_[s]); 74 | 75 | for (int s = 0; s < num_cuda_stream_; ++s) 76 | { 77 | cuda_img_[s].upload(hand_rendered_[s], cuda_stream_[s]); 78 | cuda::cvtColor(cuda_img_[s], cuda_img_alpha_[s], COLOR_BGR2BGRA, 4, cuda_stream_[s]); 79 | cuda_hog_->compute(cuda_img_alpha_[s], cuda_descriptors_[s], cuda_stream_[s]); 80 | } 81 | 82 | for (int s = 0; s < num_cuda_stream_; ++s) 83 | { 84 | cuda_stream_[s].waitForCompletion(); 85 | 86 | cuda_descriptors_[s].download(cpu_descriptors_[s], cuda_stream_[s]); 87 | 88 | for (int i = 0; i < num_img_stream_; ++i) 89 | { 90 | float sum_diff = 0; 91 | { 92 | auto it_cam = (static_cast*>(measurements.getObj()))->begin(); 93 | auto it_cam_end = (static_cast*>(measurements.getObj()))->end(); 94 | int j = 0; 95 | while (it_cam < it_cam_end) 96 | { 97 | sum_diff += abs((*it_cam) - cpu_descriptors_[s].at(i, j)); 98 | 99 | ++it_cam; 100 | ++j; 101 | } 102 | } 103 | 104 | innovations(s * num_img_stream_ + i, 0) = sum_diff; 105 | } 106 | } 107 | } 108 | 109 | 110 | double VisualUpdateParticles::likelihood(const Ref& innovations) 111 | { 112 | return exp(-likelihood_gain_ * innovations.cast().coeff(0, 0)); 113 | } 114 | 115 | 116 | bfl::VisualObservationModel& VisualUpdateParticles::getVisualObservationModel() 117 | { 118 | return *observation_model_; 119 | } 120 | 121 | 122 | void VisualUpdateParticles::setVisualObservationModel(std::unique_ptr visual_observation_model) 123 | { 124 | throw std::runtime_error("ERROR::VISUALUPDATEPARTICLES::SETVISUALOBSERVATIONMODEL\nERROR:\n\tClass under development. Do not invoke this method."); 125 | 126 | VisualObservationModel* tmp_observation_model = visual_observation_model.release(); 127 | 128 | observation_model_ = std::unique_ptr(dynamic_cast(tmp_observation_model)); 129 | } 130 | 131 | 132 | void VisualUpdateParticles::correctStep(const Ref& pred_states, const Ref& pred_weights, cv::InputArray measurements, 133 | Ref cor_states, Ref cor_weights) 134 | { 135 | VectorXf innovations(pred_states.cols()); 136 | innovation(pred_states, measurements, innovations); 137 | 138 | cor_states = pred_states; 139 | 140 | for (int i = 0; i < innovations.rows(); ++i) 141 | { 142 | cor_weights(i) = pred_weights(i) * likelihood(innovations.row(i)); 143 | 144 | if (cor_weights(i) <= 0) 145 | cor_weights(i) = std::numeric_limits::min(); 146 | } 147 | } 148 | -------------------------------------------------------------------------------- /src/hand-tracking/src/iCubFwdKinModel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | using namespace bfl; 21 | using namespace Eigen; 22 | using namespace iCub::ctrl; 23 | using namespace iCub::iKin; 24 | using namespace yarp::eigen; 25 | using namespace yarp::math; 26 | using namespace yarp::os; 27 | using namespace yarp::sig; 28 | 29 | 30 | iCubFwdKinModel::iCubFwdKinModel(const ConstString& robot, const ConstString& laterality, const ConstString& port_prefix) : 31 | icub_kin_arm_(iCubArm(laterality+"_v2")), 32 | robot_(robot), laterality_(laterality), port_prefix_(port_prefix) 33 | { 34 | Property opt_arm_enc; 35 | opt_arm_enc.put("device", "remote_controlboard"); 36 | opt_arm_enc.put("local", "/hand-tracking/" + ID_ + "/" + port_prefix + "/control_" + laterality_ + "_arm"); 37 | opt_arm_enc.put("remote", "/" + robot_ + "/right_arm"); 38 | 39 | yInfo() << log_ID_ << "Opening " + laterality_ + " arm remote_controlboard driver..."; 40 | 41 | if (drv_arm_enc_.open(opt_arm_enc)) 42 | { 43 | yInfo() << log_ID_ << "Succesfully opened " + laterality_ + " arm remote_controlboard interface."; 44 | 45 | yInfo() << log_ID_ << "Getting " + laterality_ + " arm encoder interface..."; 46 | 47 | drv_arm_enc_.view(itf_arm_enc_); 48 | if (!itf_arm_enc_) 49 | { 50 | yError() << log_ID_ << "Cannot get " + laterality_ + " arm encoder interface!"; 51 | 52 | drv_arm_enc_.close(); 53 | throw std::runtime_error("ERROR::" + ID_ + "::CTOR::INTERFACE\nERROR: cannot get " + laterality_ + " arm encoder interface!"); 54 | } 55 | 56 | yInfo() << log_ID_ << "Succesfully got " + laterality_ + " arm encoder interface."; 57 | } 58 | else 59 | { 60 | yError() << log_ID_ << "Cannot open " + laterality_ + " arm remote_controlboard!"; 61 | 62 | throw std::runtime_error("ERROR::" + ID_ + "::CTOR::DRIVER\nERROR: cannot open " + laterality_ + " arm remote_controlboard!"); 63 | } 64 | 65 | icub_kin_arm_.setAllConstraints(false); 66 | icub_kin_arm_.releaseLink(0); 67 | icub_kin_arm_.releaseLink(1); 68 | icub_kin_arm_.releaseLink(2); 69 | 70 | Property opt_torso_enc; 71 | opt_torso_enc.put("device", "remote_controlboard"); 72 | opt_torso_enc.put("local", "/hand-tracking/" + ID_ + "/" + port_prefix + "/control_torso"); 73 | opt_torso_enc.put("remote", "/" + robot_ + "/torso"); 74 | 75 | yInfo() << log_ID_ << "Opening torso remote_controlboard driver..."; 76 | 77 | if (drv_torso_enc_.open(opt_torso_enc)) 78 | { 79 | yInfo() << log_ID_ << "Succesfully opened torso remote_controlboard driver."; 80 | 81 | yInfo() << log_ID_ << "Getting torso encoder interface..."; 82 | 83 | drv_torso_enc_.view(itf_torso_enc_); 84 | if (!itf_torso_enc_) 85 | { 86 | yError() << log_ID_ << "Cannot get torso encoder interface!"; 87 | 88 | drv_torso_enc_.close(); 89 | throw std::runtime_error("ERROR::" + ID_ + "::CTOR::INTERFACE\nERROR: cannot get torso encoder interface!"); 90 | } 91 | 92 | yInfo() << log_ID_ << "Succesfully got torso encoder interface."; 93 | } 94 | else 95 | { 96 | yError() << log_ID_ << "Cannot open torso remote_controlboard!"; 97 | 98 | throw std::runtime_error("ERROR::" + ID_ + "::CTOR::DRIVER\nERROR: cannot open torso remote_controlboard!"); 99 | } 100 | 101 | yInfo() << log_ID_ << "Succesfully initialized."; 102 | } 103 | 104 | 105 | iCubFwdKinModel::~iCubFwdKinModel() noexcept 106 | { 107 | drv_arm_enc_.close(); 108 | drv_torso_enc_.close(); 109 | } 110 | 111 | 112 | bool iCubFwdKinModel::setProperty(const std::string& property) 113 | { 114 | return FwdKinModel::setProperty(property); 115 | } 116 | 117 | 118 | VectorXd iCubFwdKinModel::readPose() 119 | { 120 | Vector ee_pose = icub_kin_arm_.EndEffPose(CTRL_DEG2RAD * readRootToEE()); 121 | 122 | return toEigen(ee_pose); 123 | } 124 | 125 | 126 | Vector iCubFwdKinModel::readTorso() 127 | { 128 | int torso_enc_num; 129 | itf_torso_enc_->getAxes(&torso_enc_num); 130 | Vector enc_torso(torso_enc_num); 131 | 132 | while (!itf_torso_enc_->getEncoders(enc_torso.data())); 133 | 134 | std::swap(enc_torso(0), enc_torso(2)); 135 | 136 | return enc_torso; 137 | } 138 | 139 | 140 | Vector iCubFwdKinModel::readRootToEE() 141 | { 142 | int arm_enc_num; 143 | itf_arm_enc_->getAxes(&arm_enc_num); 144 | Vector enc_arm(arm_enc_num); 145 | 146 | Vector root_ee_enc(10); 147 | 148 | root_ee_enc.setSubvector(0, readTorso()); 149 | 150 | while (!itf_arm_enc_->getEncoders(enc_arm.data())); 151 | for (size_t i = 0; i < 7; ++i) 152 | root_ee_enc(i + 3) = enc_arm(i); 153 | 154 | return root_ee_enc; 155 | } 156 | -------------------------------------------------------------------------------- /src/hand-tracking/src/iCubGatePose.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | using namespace bfl; 16 | using namespace cv; 17 | using namespace Eigen; 18 | using namespace iCub::ctrl; 19 | using namespace iCub::iKin; 20 | using namespace yarp::eigen; 21 | using namespace yarp::math; 22 | using namespace yarp::os; 23 | using namespace yarp::sig; 24 | 25 | 26 | iCubGatePose::iCubGatePose(std::unique_ptr visual_correction, 27 | const double gate_x, const double gate_y, const double gate_z, 28 | const double gate_rotation, 29 | const double gate_aperture, 30 | const yarp::os::ConstString& robot, const yarp::os::ConstString& laterality, const yarp::os::ConstString& port_prefix) : 31 | GatePose(std::move(visual_correction), 32 | gate_x, gate_y, gate_z, 33 | gate_rotation, 34 | gate_aperture), 35 | icub_kin_arm_(iCubArm(laterality + "_v2")), robot_(robot), laterality_(laterality), port_prefix_(port_prefix) 36 | { 37 | icub_kin_arm_.setAllConstraints(false); 38 | icub_kin_arm_.releaseLink(0); 39 | icub_kin_arm_.releaseLink(1); 40 | icub_kin_arm_.releaseLink(2); 41 | 42 | 43 | Property opt_arm_enc; 44 | opt_arm_enc.put("device", "remote_controlboard"); 45 | opt_arm_enc.put("local", "/hand-tracking/" + ID_ + "/" + port_prefix + "/control_" + laterality_ + "_arm"); 46 | opt_arm_enc.put("remote", "/" + robot_ + "/" + laterality_ + "_arm"); 47 | 48 | yInfo() << log_ID_ << "Opening " + laterality_ + " arm remote_controlboard driver..."; 49 | if (drv_arm_enc_.open(opt_arm_enc)) 50 | { 51 | yInfo() << log_ID_ << "Succesfully opened " + laterality_ + " arm remote_controlboard interface."; 52 | 53 | yInfo() << log_ID_ << "Getting " + laterality_ + " arm encoder interface..."; 54 | 55 | drv_arm_enc_.view(itf_arm_enc_); 56 | if (!itf_arm_enc_) 57 | { 58 | yError() << log_ID_ << "Cannot get " + laterality_ + " arm encoder interface!"; 59 | 60 | drv_arm_enc_.close(); 61 | throw std::runtime_error("ERROR::" + ID_ + "::CTOR::INTERFACE\nERROR: cannot get " + laterality_ + " arm encoder interface!"); 62 | } 63 | 64 | yInfo() << log_ID_ << "Succesfully got " + laterality_ + " arm encoder interface."; 65 | } 66 | else 67 | { 68 | yError() << log_ID_ << "Cannot open " + laterality_ + " arm remote_controlboard!"; 69 | 70 | throw std::runtime_error("ERROR::" + ID_ + "::CTOR::DRIVER\nERROR: cannot open " + laterality_ + " arm remote_controlboard!"); 71 | } 72 | 73 | 74 | Property opt_torso_enc; 75 | opt_torso_enc.put("device", "remote_controlboard"); 76 | opt_torso_enc.put("local", "/hand-tracking/" + ID_ + "/" + port_prefix + "/control_torso"); 77 | opt_torso_enc.put("remote", "/" + robot_ + "/torso"); 78 | 79 | yInfo() << log_ID_ << "Opening torso remote_controlboard driver..."; 80 | 81 | if (drv_torso_enc_.open(opt_torso_enc)) 82 | { 83 | yInfo() << log_ID_ << "Succesfully opened torso remote_controlboard driver."; 84 | 85 | yInfo() << log_ID_ << "Getting torso encoder interface..."; 86 | 87 | drv_torso_enc_.view(itf_torso_enc_); 88 | if (!itf_torso_enc_) 89 | { 90 | yError() << log_ID_ << "Cannot get torso encoder interface!"; 91 | 92 | drv_torso_enc_.close(); 93 | throw std::runtime_error("ERROR::" + ID_ + "::CTOR::INTERFACE\nERROR: cannot get torso encoder interface!"); 94 | } 95 | 96 | yInfo() << log_ID_ << "Succesfully got torso encoder interface."; 97 | } 98 | else 99 | { 100 | yError() << log_ID_ << "Cannot open torso remote_controlboard!"; 101 | 102 | throw std::runtime_error("ERROR::" + ID_ + "::CTOR::DRIVER\nERROR: cannot open torso remote_controlboard!"); 103 | } 104 | 105 | yInfo() << log_ID_ << "Succesfully initialized."; 106 | } 107 | 108 | 109 | iCubGatePose::iCubGatePose(std::unique_ptr visual_correction, 110 | const yarp::os::ConstString& robot, const yarp::os::ConstString& laterality, const yarp::os::ConstString& port_prefix) : 111 | iCubGatePose(std::move(visual_correction), 0.1, 0.1, 0.1, 5, 30, robot, laterality, port_prefix) { } 112 | 113 | 114 | iCubGatePose::~iCubGatePose() noexcept { } 115 | 116 | 117 | Vector iCubGatePose::readTorso() 118 | { 119 | int torso_enc_num; 120 | itf_torso_enc_->getAxes(&torso_enc_num); 121 | Vector enc_torso(torso_enc_num); 122 | 123 | while (!itf_torso_enc_->getEncoders(enc_torso.data())); 124 | 125 | std::swap(enc_torso(0), enc_torso(2)); 126 | 127 | return enc_torso; 128 | } 129 | 130 | 131 | Vector iCubGatePose::readRootToEE() 132 | { 133 | int arm_enc_num; 134 | itf_arm_enc_->getAxes(&arm_enc_num); 135 | Vector enc_arm(arm_enc_num); 136 | 137 | Vector root_ee_enc(10); 138 | 139 | root_ee_enc.setSubvector(0, readTorso()); 140 | 141 | while (!itf_arm_enc_->getEncoders(enc_arm.data())); 142 | 143 | for (size_t i = 0; i < 7; ++i) 144 | root_ee_enc(i + 3) = enc_arm(i); 145 | 146 | return root_ee_enc; 147 | } 148 | 149 | 150 | VectorXd iCubGatePose::readPose() 151 | { 152 | Vector ee_pose = icub_kin_arm_.EndEffPose(CTRL_DEG2RAD * readRootToEE()); 153 | 154 | return toEigen(ee_pose); 155 | } 156 | -------------------------------------------------------------------------------- /src/hand-tracking/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | using namespace bfl; 34 | using namespace cv; 35 | using namespace Eigen; 36 | using namespace yarp::dev; 37 | using namespace yarp::os; 38 | 39 | 40 | /* UTILITY FUNCTIONS FOREWORD DECLARATIONS */ 41 | std::string engine_count_to_string(int engine_count); 42 | 43 | 44 | /* MAIN */ 45 | int main(int argc, char *argv[]) 46 | { 47 | ConstString log_ID = "[Main]"; 48 | yInfo() << log_ID << "Configuring and starting module..."; 49 | 50 | Network yarp; 51 | if (!yarp.checkNetwork(3.0)) 52 | { 53 | yError() << "YARP seems unavailable!"; 54 | return EXIT_FAILURE; 55 | } 56 | 57 | 58 | // Page locked dovrebbe essere più veloce da utilizzate con CUDA, non sembra essere il caso. 59 | // Mat::setDefaultAllocator(cuda::HostMem::getAllocator(cuda::HostMem::PAGE_LOCKED)); 60 | 61 | cuda::DeviceInfo gpu_dev; 62 | yInfo() << log_ID << "[CUDA] Engine capability:" << engine_count_to_string(gpu_dev.asyncEngineCount()); 63 | yInfo() << log_ID << "[CUDA] Can have concurrent kernel:" << gpu_dev.concurrentKernels(); 64 | yInfo() << log_ID << "[CUDA] Streaming multiprocessor count:" << gpu_dev.multiProcessorCount(); 65 | yInfo() << log_ID << "[CUDA] Can map host memory:" << gpu_dev.canMapHostMemory(); 66 | yInfo() << log_ID << "[CUDA] Clock:" << gpu_dev.clockRate() << "KHz"; 67 | 68 | 69 | ResourceFinder rf; 70 | rf.setVerbose(); 71 | rf.setDefaultContext("hand-tracking"); 72 | rf.setDefaultConfigFile("config.ini"); 73 | rf.configure(argc, argv); 74 | 75 | FilteringParamtersD paramsd; 76 | paramsd["num_particles"] = rf.findGroup("PF").check("num_particles", Value(50)).asInt(); 77 | paramsd["gpu_count"] = rf.findGroup("PF").check("gpu_count", Value(1.0)).asInt(); 78 | paramsd["resample_prior"] = rf.findGroup("PF").check("resample_prior", Value(1.0)).asInt(); 79 | paramsd["gate_pose"] = rf.findGroup("PF").check("gate_pose", Value(0.0)).asInt(); 80 | paramsd["resolution_ratio"] = rf.findGroup("PF").check("resolution_ratio", Value(1.0)).asInt(); 81 | paramsd["num_images"] = paramsd["num_particles"] / paramsd["gpu_count"]; 82 | if (rf.check("play")) 83 | paramsd["play"] = 1.0; 84 | else 85 | paramsd["play"] = rf.findGroup("PF").check("play", Value(1.0)).asDouble(); 86 | 87 | paramsd["q_xy"] = rf.findGroup("BROWNIANMOTION").check("q_xy", Value(0.005)).asDouble(); 88 | paramsd["q_z"] = rf.findGroup("BROWNIANMOTION").check("q_z", Value(0.005)).asDouble(); 89 | paramsd["theta"] = rf.findGroup("BROWNIANMOTION").check("theta", Value(3.0)).asDouble(); 90 | paramsd["cone_angle"] = rf.findGroup("BROWNIANMOTION").check("cone_angle", Value(2.5)).asDouble(); 91 | paramsd["seed"] = rf.findGroup("BROWNIANMOTION").check("seed", Value(1.0)).asDouble(); 92 | 93 | paramsd["use_thumb"] = rf.findGroup("VISUALPROPRIOCEPTION").check("use_thumb", Value(0.0)).asDouble(); 94 | paramsd["use_forearm"] = rf.findGroup("VISUALPROPRIOCEPTION").check("use_forearm", Value(0.0)).asDouble(); 95 | 96 | paramsd["likelihood_gain"] = rf.findGroup("VISUALUPDATEPARTICLES").check("likelihood_gain", Value(0.001)).asDouble(); 97 | 98 | paramsd["gate_x"] = rf.findGroup("GATEPOSE").check("gate_x", Value(0.1)).asDouble(); 99 | paramsd["gate_y"] = rf.findGroup("GATEPOSE").check("gate_y", Value(0.1)).asDouble(); 100 | paramsd["gate_z"] = rf.findGroup("GATEPOSE").check("gate_z", Value(0.1)).asDouble(); 101 | paramsd["gate_aperture"] = rf.findGroup("GATEPOSE").check("gate_aperture", Value(15.0)).asDouble(); 102 | paramsd["gate_rotation"] = rf.findGroup("GATEPOSE").check("gate_rotation", Value(30.0)).asDouble(); 103 | 104 | paramsd["resample_ratio"] = rf.findGroup("RESAMPLING").check("resample_ratio", Value(0.3)).asDouble(); 105 | paramsd["prior_ratio"] = rf.findGroup("RESAMPLING").check("prior_ratio", Value(0.5)).asDouble(); 106 | 107 | FilteringParamtersS paramss; 108 | paramss["robot"] = rf.findGroup("PF").check("robot", Value("icub")).asString(); 109 | if (rf.check("cam")) 110 | paramss["cam_sel"] = rf.find("cam").asString(); 111 | else 112 | paramss["cam_sel"] = rf.findGroup("PF").check("cam_sel", Value("left")).asString(); 113 | paramss["laterality"] = rf.findGroup("PF").check("laterality", Value("right")).asString(); 114 | 115 | 116 | yInfo() << log_ID << "Running with:"; 117 | yInfo() << log_ID << " - robot:" << paramss["robot"]; 118 | yInfo() << log_ID << " - cam_sel:" << paramss["cam_sel"]; 119 | yInfo() << log_ID << " - laterality:" << paramss["laterality"]; 120 | 121 | yInfo() << log_ID << " - num_particles:" << paramsd["num_particles"]; 122 | yInfo() << log_ID << " - gpu_count:" << paramsd["gpu_count"]; 123 | yInfo() << log_ID << " - num_images:" << paramsd["num_images"]; 124 | yInfo() << log_ID << " - resample_prior:" << paramsd["resample_prior"]; 125 | yInfo() << log_ID << " - gate_pose:" << paramsd["gate_pose"]; 126 | yInfo() << log_ID << " - play:" << (paramsd["play"] == 1.0 ? "true" : "false"); 127 | 128 | yInfo() << log_ID << " - q_xy:" << paramsd["q_xy"]; 129 | yInfo() << log_ID << " - q_z:" << paramsd["q_z"]; 130 | yInfo() << log_ID << " - theta:" << paramsd["theta"]; 131 | yInfo() << log_ID << " - cone_angle:" << paramsd["cone_angle"]; 132 | yInfo() << log_ID << " - seed:" << paramsd["seed"]; 133 | 134 | yInfo() << log_ID << " - use_thumb:" << paramsd["use_thumb"]; 135 | yInfo() << log_ID << " - use_forearm:" << paramsd["use_forearm"]; 136 | 137 | yInfo() << log_ID << " - likelihood_gain:" << paramsd["likelihood_gain"]; 138 | 139 | yInfo() << log_ID << " - resample_ratio:" << paramsd["resample_ratio"]; 140 | yInfo() << log_ID << " - prior_ratio:" << paramsd["prior_ratio"]; 141 | 142 | yInfo() << log_ID << " - gate_x:" << paramsd["gate_x"]; 143 | yInfo() << log_ID << " - gate_y:" << paramsd["gate_y"]; 144 | yInfo() << log_ID << " - gate_z:" << paramsd["gate_z"]; 145 | yInfo() << log_ID << " - gate_aperture:" << paramsd["gate_aperture"]; 146 | yInfo() << log_ID << " - gate_rotation:" << paramsd["gate_rotation"]; 147 | 148 | 149 | /* INITIALIZATION */ 150 | std::unique_ptr init_arm(new InitiCubArm("hand-tracking/InitiCubArm", paramss["cam_sel"], paramss["laterality"])); 151 | 152 | 153 | /* MOTION MODEL */ 154 | std::unique_ptr brown(new BrownianMotionPose(paramsd["q_xy"], paramsd["q_z"], paramsd["theta"], paramsd["cone_angle"], paramsd["seed"])); 155 | 156 | std::unique_ptr icub_motion; 157 | if (paramsd["play"] != 1.0) 158 | { 159 | std::unique_ptr icub_fwdkin(new iCubFwdKinModel(paramss["robot"], paramss["laterality"], paramss["cam_sel"])); 160 | icub_motion = std::move(icub_fwdkin); 161 | } 162 | else 163 | { 164 | std::unique_ptr play_fwdkin(new PlayFwdKinModel(paramss["robot"], paramss["laterality"], paramss["cam_sel"])); 165 | icub_motion = std::move(play_fwdkin); 166 | } 167 | 168 | /* PREDICTION */ 169 | std::unique_ptr pf_prediction(new DrawFwdKinPoses()); 170 | pf_prediction->setStateModel(std::move(brown)); 171 | pf_prediction->setExogenousModel(std::move(icub_motion)); 172 | 173 | 174 | /* SENSOR MODEL */ 175 | std::unique_ptr proprio; 176 | try 177 | { 178 | std::unique_ptr vp(new VisualProprioception(paramsd["use_thumb"], 179 | paramsd["use_forearm"], 180 | paramsd["num_images"], 181 | paramsd["resolution_ratio"], 182 | paramss["cam_sel"], 183 | paramss["laterality"], 184 | rf.getContext())); 185 | 186 | proprio = std::move(vp); 187 | paramsd["num_particles"] = proprio->getOGLTilesRows() * proprio->getOGLTilesCols() * paramsd["gpu_count"]; 188 | paramsd["cam_width"] = proprio->getCamWidth(); 189 | paramsd["cam_height"] = proprio->getCamHeight(); 190 | } 191 | catch (const std::runtime_error& e) 192 | { 193 | yError() << e.what(); 194 | return EXIT_FAILURE; 195 | } 196 | 197 | /* CORRECTION */ 198 | std::unique_ptr vpf_correction; 199 | 200 | std::unique_ptr vpf_update_particles(new VisualUpdateParticles(std::move(proprio), paramsd["likelihood_gain"], paramsd["gpu_count"])); 201 | 202 | if (paramsd["gate_pose"] == 1.0) 203 | { 204 | if (paramsd["play"] != 1.0) 205 | { 206 | std::unique_ptr icub_gate_pose(new iCubGatePose(std::move(vpf_update_particles), 207 | paramsd["gate_x"], paramsd["gate_y"], paramsd["gate_z"], 208 | paramsd["gate_aperture"], paramsd["gate_rotation"], 209 | paramss["robot"], paramss["laterality"], paramss["cam_sel"])); 210 | vpf_correction = std::move(icub_gate_pose); 211 | } 212 | else 213 | { 214 | std::unique_ptr icub_gate_pose(new PlayGatePose(std::move(vpf_update_particles), 215 | paramsd["gate_x"], paramsd["gate_y"], paramsd["gate_z"], 216 | paramsd["gate_aperture"], paramsd["gate_rotation"], 217 | paramss["robot"], paramss["laterality"], paramss["cam_sel"])); 218 | vpf_correction = std::move(icub_gate_pose); 219 | } 220 | } 221 | else 222 | vpf_correction = std::move(vpf_update_particles); 223 | 224 | 225 | /* RESAMPLING */ 226 | std::unique_ptr pf_resampling; 227 | if (paramsd["resample_prior"] != 1.0) 228 | { 229 | std::unique_ptr resampling(new Resampling()); 230 | 231 | pf_resampling = std::move(resampling); 232 | } 233 | else 234 | { 235 | std::unique_ptr resample_init_arm(new InitiCubArm("hand-tracking/ResamplingWithPrior/InitiCubArm", paramss["cam_sel"], paramss["laterality"])); 236 | std::unique_ptr resampling_prior(new ResamplingWithPrior(std::move(resample_init_arm), paramsd["prior_ratio"])); 237 | 238 | pf_resampling = std::move(resampling_prior); 239 | } 240 | 241 | 242 | /* PARTICLE FILTER */ 243 | VisualSIS vsis_pf(paramss["cam_sel"], 244 | paramsd["cam_width"], paramsd["cam_height"], 245 | paramsd["num_particles"], 246 | paramsd["resample_ratio"]); 247 | vsis_pf.setInitialization(std::move(init_arm)); 248 | vsis_pf.setPrediction(std::move(pf_prediction)); 249 | vsis_pf.setCorrection(std::move(vpf_correction)); 250 | vsis_pf.setResampling(std::move(pf_resampling)); 251 | 252 | 253 | vsis_pf.boot(); 254 | vsis_pf.wait(); 255 | 256 | 257 | yInfo() << log_ID << "Application closed succesfully."; 258 | return EXIT_SUCCESS; 259 | } 260 | 261 | 262 | /* UTILITY FUNCTIONS */ 263 | std::string engine_count_to_string(int engine_count) 264 | { 265 | if (engine_count == 0) return "concurrency is unsupported on this device"; 266 | if (engine_count == 1) return "the device can concurrently copy memory between host and device while executing a kernel"; 267 | if (engine_count == 2) return "the device can concurrently copy memory between host and device in both directions and execute a kernel at the same time"; 268 | return "wrong argument...!"; 269 | } 270 | -------------------------------------------------------------------------------- /src/hand-tracking/thrift/visualsisparticlefilter.thrift: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | /** 9 | * VisualSISParticleFilterIDL 10 | * 11 | * IDL Interface to \ref VisualSIRParticleFilter options. 12 | */ 13 | service VisualSISParticleFilterIDL 14 | { 15 | /** 16 | * Initialize and run the visual SIR particle filter. Returns upon 17 | * successful or failure setup. 18 | * 19 | * @return true/false on success/failure. 20 | */ 21 | bool run_filter(); 22 | 23 | /** 24 | * Reset the visual SIR particle filter. Returns upon successful or failure 25 | * reset. 26 | * 27 | * @return true/false on success/failure. 28 | */ 29 | bool reset_filter(); 30 | 31 | /** 32 | * Stop and reset the SIR particle filter. This method must be called when 33 | * the SIR particle filter is no longer needed or a new filtering task 34 | * need to be initialized. 35 | * 36 | * @return true/false on success/failure. 37 | */ 38 | bool stop_filter(); 39 | 40 | /** 41 | * Enable/Disable skipping the filtering step specified in what_step. 42 | * what_step can be one of the following: 43 | * 44 | * 1) prediction: skips the whole prediction step 45 | * 46 | * 2) state: skips the prediction step related to the state transition 47 | * 48 | * 3) exogenous: skips the prediction step related exogenous inputs 49 | * 50 | * 4) correction: skips the whole correction step 51 | * 52 | * @param what_step the step to skipping 53 | * @param status enable/disable skipping 54 | * 55 | * @return true/false on success/failure. 56 | */ 57 | bool skip_step(1:string what_step, 2:bool status); 58 | 59 | /** 60 | * Use/Don't use the analog values from the right hand to correct the finger 61 | * poses. 62 | * 63 | * @param status true/false to use/don't use analog values. 64 | * 65 | * @return true activation/deactivation success, false otherwise. 66 | */ 67 | bool use_analogs(1:bool status); 68 | 69 | /** 70 | * Get information about recursive Bayesian filter, like it's status, the 71 | * available methods, and the current one in use, to extract the state 72 | * estimate from the particle set. 73 | * 74 | * @return a string with all the available information, 'none' otherwise 75 | */ 76 | list get_info(); 77 | 78 | /** 79 | * Change the current method to extract the state estimates from the 80 | * particle set. 81 | * 82 | * @param status a string with the state estimate extraction method to use; 83 | * the string shall be one of the available methods returned 84 | * by the get_info() method. 85 | * 86 | * @return true method changed successfully, false otherwise. 87 | */ 88 | bool set_estimates_extraction_method(1:string method); 89 | 90 | /** 91 | * Change the window size of mobile averages for estimates extraction. 92 | * 93 | * @param window specifies the mobile window size. 94 | * 95 | * @return true window size changed successfully, false otherwise. 96 | * 97 | * @note The default value is 20. Minimum value is 2. Maximum value is 90. 98 | */ 99 | bool set_mobile_average_window(1:i16 window = 20); 100 | 101 | /** 102 | * Gently close the application, deallocating resources. 103 | */ 104 | bool quit(); 105 | } 106 | -------------------------------------------------------------------------------- /src/reaching/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # BSD 3-Clause license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | set(EXE_TARGET_NAME reaching) 11 | 12 | find_package(glew REQUIRED) 13 | find_package(ICUB REQUIRED) 14 | find_package(OpenCV REQUIRED) 15 | find_package(SuperimposeMesh REQUIRED) 16 | find_package(YARP REQUIRED) 17 | 18 | LIST(APPEND CMAKE_MODULE_PATH ${YARP_MODULE_PATH} 19 | ${ICUB_MODULE_PATH}) 20 | 21 | include(YarpInstallationHelpers) 22 | 23 | # Application source and header files 24 | set(${EXE_TARGET_NAME}_SRC 25 | main.cpp) 26 | 27 | add_executable(${EXE_TARGET_NAME} ${${EXE_TARGET_NAME}_SRC}) 28 | 29 | target_include_directories(${EXE_TARGET_NAME} PRIVATE "$" 30 | ${ICUB_INCLUDE_DIRS} 31 | ${YARP_INCLUDE_DIRS}) 32 | 33 | target_link_libraries(${EXE_TARGET_NAME} ctrlLib 34 | ${ICUB_LIBRARIES} 35 | iKin 36 | ${OpenCV_LIBS} 37 | SuperimposeMesh::SuperimposeMesh 38 | ${YARP_LIBRARIES}) 39 | 40 | set(${EXE_TARGET_NAME}_APP 41 | ${PROJECT_SOURCE_DIR}/app/reaching.xml 42 | ${PROJECT_SOURCE_DIR}/app/reaching-SIM.xml) 43 | 44 | install(TARGETS ${EXE_TARGET_NAME} DESTINATION bin) 45 | install(FILES ${${EXE_TARGET_NAME}_APP} DESTINATION ${ICUBCONTRIB_APPLICATIONS_INSTALL_DIR}) 46 | -------------------------------------------------------------------------------- /src/visualservoing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # BSD 3-Clause license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | set(PLUGIN_NAME visualservoing) 11 | 12 | find_package(YARP REQUIRED) 13 | 14 | list(APPEND CMAKE_MODULE_PATH ${YARP_MODULE_PATH}) 15 | include(YarpPlugin) 16 | yarp_configure_external_installation(${PLUGIN_NAME} WITH_PLUGINS) 17 | 18 | yarp_begin_plugin_library(visualservoingplugin) 19 | option(BUILD_VISUAL_SERVOING_CLIENT "Build visualservoing-client YARP plugin" OFF) 20 | if(BUILD_VISUAL_SERVOING_CLIENT) 21 | add_subdirectory(visualservoingclient) 22 | add_subdirectory(visualservoingclient-app) 23 | endif() 24 | 25 | option(BUILD_VISUAL_SERVOING_SERVER "Build visualservoing-server YARP plugin" OFF) 26 | if(BUILD_VISUAL_SERVOING_SERVER) 27 | add_subdirectory(visualservoingserver) 28 | add_subdirectory(visualservoingserver-app) 29 | endif() 30 | 31 | 32 | # Application XML app files 33 | if(BUILD_VISUAL_SERVOING_CLIENT OR BUILD_VISUAL_SERVOING_SERVER) 34 | set(${PLUGIN_NAME}_APP 35 | visualservoingcommon/app/BackgroundApps-SIM.xml 36 | visualservoingcommon/app/visualservoing.xml 37 | visualservoingcommon/app/visualservoing-click.xml 38 | visualservoingcommon/app/visualservoing-SIM.xml 39 | visualservoingcommon/app/visualservoing-datadump.xml) 40 | 41 | install(FILES ${${PLUGIN_NAME}_APP} DESTINATION ${ICUBCONTRIB_APPLICATIONS_INSTALL_DIR}) 42 | endif() 43 | yarp_end_plugin_library(visualservoingplugin) 44 | -------------------------------------------------------------------------------- /src/visualservoing/visualservoingclient-app/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # BSD 3-Clause license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | set(APP_TARGET_NAME visualservoingclient_app) 11 | 12 | find_package(YARP REQUIRED) 13 | 14 | include(YarpInstallationHelpers) 15 | 16 | LIST(APPEND CMAKE_MODULE_PATH ${YARP_MODULE_PATH}) 17 | 18 | include(YarpInstallationHelpers) 19 | 20 | # Application source and header files 21 | set(${APP_TARGET_NAME}_SRC 22 | src/main.cpp) 23 | 24 | # Application target calls 25 | add_executable(${APP_TARGET_NAME} ${${APP_TARGET_NAME}_SRC}) 26 | 27 | target_include_directories(${APP_TARGET_NAME} PRIVATE ${YARP_INCLUDE_DIRS}) 28 | 29 | target_link_libraries(${APP_TARGET_NAME} PRIVATE ${YARP_LIBRARIES} 30 | visualservoingplugin) 31 | 32 | install(TARGETS ${APP_TARGET_NAME} DESTINATION bin) 33 | -------------------------------------------------------------------------------- /src/visualservoing/visualservoingclient-app/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | using namespace yarp::os; 16 | using namespace yarp::sig; 17 | using namespace yarp::dev; 18 | using namespace yarp::math; 19 | 20 | YARP_DECLARE_PLUGINS(visualservoingplugin); 21 | 22 | int main(int argc, char **argv) 23 | { 24 | Network yarp; 25 | if (!yarp.checkNetwork(3.0)) 26 | { 27 | yError("YARP seems unavailable!"); 28 | return EXIT_FAILURE; 29 | } 30 | 31 | YARP_REGISTER_PLUGINS(visualservoingplugin); 32 | 33 | Property prop_client_vs; 34 | prop_client_vs.put("device", "visualservoingclient"); 35 | prop_client_vs.put("verbosity", true); 36 | prop_client_vs.put("local", "/VisualServoingClientTest"); 37 | prop_client_vs.put("remote", "/visualservoing"); 38 | bool use_fwd_kin = false; 39 | 40 | PolyDriver drv_client_vs(prop_client_vs); 41 | if (!drv_client_vs.isValid()) 42 | { 43 | yError("Could not run VisualServoingClient!"); 44 | return EXIT_FAILURE; 45 | } 46 | 47 | IVisualServoing *visual_servoing; 48 | drv_client_vs.view(visual_servoing); 49 | if (visual_servoing == YARP_NULLPTR) 50 | { 51 | yError("Could not get interfacate to VisualServoingClient!"); 52 | return EXIT_FAILURE; 53 | } 54 | 55 | /* Set visual servoing control*/ 56 | visual_servoing->setVisualServoControl("decoupled"); 57 | 58 | /* Stored set-up: t170904 */ 59 | visual_servoing->storedInit("t170904"); 60 | 61 | visual_servoing->initFacilities(use_fwd_kin); 62 | visual_servoing->storedGoToGoal("t170904"); 63 | 64 | visual_servoing->checkVisualServoingController(); 65 | visual_servoing->waitVisualServoingDone(); 66 | 67 | visual_servoing->stopFacilities(); 68 | 69 | /* Stored set-up: t170427 */ 70 | visual_servoing->storedInit("t170427"); 71 | 72 | visual_servoing->initFacilities(use_fwd_kin); 73 | visual_servoing->storedGoToGoal("t170427"); 74 | 75 | visual_servoing->checkVisualServoingController(); 76 | visual_servoing->waitVisualServoingDone(); 77 | 78 | visual_servoing->stopFacilities(); 79 | 80 | return EXIT_SUCCESS; 81 | } 82 | -------------------------------------------------------------------------------- /src/visualservoing/visualservoingclient/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # BSD 3-Clause license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | set(PLUGIN_TARGET_NAME visualservoingclient) 11 | string(TOUPPER ${PLUGIN_NAME} UC_PLUGIN_NAME) 12 | 13 | yarp_prepare_plugin(${PLUGIN_TARGET_NAME} CATEGORY device 14 | TYPE VisualServoingClient 15 | INCLUDE VisualServoingClient.h 16 | DEFAULT ON 17 | ADVANCED 18 | INTERNAL) 19 | 20 | 21 | find_package(ICUB REQUIRED) 22 | find_package(OpenCV REQUIRED) 23 | 24 | LIST(APPEND CMAKE_MODULE_PATH ${YARP_MODULE_PATH} 25 | ${ICUB_MODULE_PATH}) 26 | 27 | include(YarpInstallationHelpers) 28 | 29 | # Plugin source and header files 30 | set(${PLUGIN_TARGET_NAME}_HDR 31 | ../visualservoingcommon/include/VisualServoingCommon.h 32 | include/VisualServoingClient.h) 33 | 34 | set(${PLUGIN_TARGET_NAME}_SRC 35 | src/VisualServoingClient.cpp) 36 | 37 | set(${PLUGIN_TARGET_NAME}_CONF 38 | conf/visualservoingclient.ini) 39 | 40 | set(${PLUGIN_TARGET_NAME}_THRIFT_HDR 41 | thrift/visualservoing.thrift) 42 | 43 | # Plugin target calls 44 | yarp_add_idl(${PLUGIN_TARGET_NAME}_THRIFT_SRC ${${PLUGIN_TARGET_NAME}_THRIFT_HDR}) 45 | 46 | yarp_add_plugin(${PLUGIN_TARGET_NAME} ${${PLUGIN_TARGET_NAME}_HDR} ${${PLUGIN_TARGET_NAME}_SRC} 47 | ${${PLUGIN_TARGET_NAME}_THRIFT_SRC}) 48 | 49 | target_include_directories(${PLUGIN_TARGET_NAME} PRIVATE "$" 50 | ${ICUB_INCLUDE_DIRS} 51 | ${YARP_INCLUDE_DIRS}) 52 | 53 | target_link_libraries(${PLUGIN_TARGET_NAME} PRIVATE ctrlLib 54 | ${ICUB_LIBRARIES} 55 | iKin 56 | ${OpenCV_LIBS} 57 | ${YARP_LIBRARIES}) 58 | 59 | yarp_install(TARGETS ${PLUGIN_TARGET_NAME} 60 | COMPONENT plugin 61 | LIBRARY DESTINATION ${ICUBCONTRIB_DYNAMIC_PLUGINS_INSTALL_DIR}) 62 | 63 | yarp_install(FILES ${${PLUGIN_TARGET_NAME}_CONF} 64 | COMPONENT plugin 65 | DESTINATION ${ICUBCONTRIB_PLUGIN_MANIFESTS_INSTALL_DIR}) 66 | -------------------------------------------------------------------------------- /src/visualservoing/visualservoingclient/conf/visualservoingclient.ini: -------------------------------------------------------------------------------- 1 | [plugin visualservoingclient] 2 | type device 3 | name visualservoingclient 4 | library visualservoingclient 5 | -------------------------------------------------------------------------------- /src/visualservoing/visualservoingclient/include/VisualServoingClient.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef VISUALSERVOINGCLIENT_H 9 | #define VISUALSERVOINGCLIENT_H 10 | 11 | #include "thrift/VisualServoingIDL.h" 12 | 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | 26 | class VisualServoingClient : public yarp::dev::DeviceDriver, 27 | public yarp::dev::IVisualServoing 28 | { 29 | public: 30 | /* Ctors and Dtors */ 31 | VisualServoingClient(); 32 | 33 | ~VisualServoingClient(); 34 | 35 | 36 | /* DeviceDriver overrides */ 37 | bool open(yarp::os::Searchable &config) override; 38 | 39 | bool close() override; 40 | 41 | 42 | /* IVisualServoing overrides */ 43 | bool initFacilities(const bool use_direct_kin) override; 44 | 45 | bool resetFacilities() override; 46 | 47 | bool stopFacilities() override; 48 | 49 | bool goToGoal(const yarp::sig::Vector& vec_x, const yarp::sig::Vector& vec_o) override; 50 | 51 | bool goToGoal(const std::vector& vec_px_l, const std::vector& vec_px_r) override; 52 | 53 | bool setModality(const std::string& mode) override; 54 | 55 | bool setVisualServoControl(const std::string& control) override; 56 | 57 | bool setControlPoint(const yarp::os::ConstString& point) override; 58 | 59 | bool getVisualServoingInfo(yarp::os::Bottle& info) override; 60 | 61 | bool setGoToGoalTolerance(const double tol = 15.0) override; 62 | 63 | bool checkVisualServoingController() override; 64 | 65 | bool waitVisualServoingDone(const double period = 0.1, const double timeout = 0.0) override; 66 | 67 | bool stopController() override; 68 | 69 | bool setTranslationGain(const double K_x_1 = 1.0, const double K_x_2 = 0.25) override; 70 | 71 | bool setMaxTranslationVelocity(const double max_x_dot) override; 72 | 73 | bool setTranslationGainSwitchTolerance(const double K_x_tol = 30.0) override; 74 | 75 | bool setOrientationGain(const double K_o_1 = 1.5, const double K_o_2 = 0.375) override; 76 | 77 | bool setMaxOrientationVelocity(const double max_o_dot) override; 78 | 79 | bool setOrientationGainSwitchTolerance(const double K_o_tol = 30.0) override; 80 | 81 | std::vector get3DGoalPositionsFrom3DPose(const yarp::sig::Vector& x, const yarp::sig::Vector& o) override; 82 | 83 | std::vector getGoalPixelsFrom3DPose(const yarp::sig::Vector& x, const yarp::sig::Vector& o, const CamSel& cam) override; 84 | 85 | bool storedInit(const std::string& label) override; 86 | 87 | bool storedGoToGoal(const std::string& label) override; 88 | 89 | bool goToSFMGoal() override; 90 | 91 | 92 | private: 93 | bool verbosity_ = false; 94 | yarp::os::ConstString local_ = ""; 95 | yarp::os::ConstString remote_ = ""; 96 | 97 | VisualServoingIDL visualservoing_control; 98 | yarp::os::Port port_rpc_command_; 99 | 100 | void yInfoVerbose (const yarp::os::ConstString& str) const { if(verbosity_) yInfo() << str; }; 101 | void yWarningVerbose(const yarp::os::ConstString& str) const { if(verbosity_) yWarning() << str; }; 102 | void yErrorVerbose (const yarp::os::ConstString& str) const { if(verbosity_) yError() << str; }; 103 | }; 104 | 105 | #endif /* VISUALSERVOINGCLIENT_H */ 106 | -------------------------------------------------------------------------------- /src/visualservoing/visualservoingclient/src/VisualServoingClient.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include "VisualServoingClient.h" 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | using namespace yarp::dev; 21 | using namespace yarp::math; 22 | using namespace yarp::os; 23 | using namespace yarp::sig; 24 | using namespace iCub::ctrl; 25 | 26 | 27 | /* Ctors and Dtors */ 28 | VisualServoingClient::VisualServoingClient() 29 | { 30 | yInfo() << "*** Invoked VisualServoingClient ctor ***"; 31 | yInfo() << "*** VisualServoingClient constructed ***"; 32 | } 33 | 34 | 35 | VisualServoingClient::~VisualServoingClient() 36 | { 37 | yInfo() << "*** Invoked VisualServoingClient dtor ***"; 38 | yInfo() << "*** VisualServoingClient destructed ***"; 39 | } 40 | 41 | 42 | /* DeviceDriver overrides */ 43 | bool VisualServoingClient::open(Searchable &config) 44 | { 45 | verbosity_ = config.check("verbosity", Value(false)).asBool(); 46 | yInfo() << "|> Verbosity: " + ConstString(verbosity_? "ON" : "OFF"); 47 | 48 | 49 | yInfoVerbose("*** Configuring VisualServoingClient ***"); 50 | 51 | local_ = config.check("local", Value("")).asString(); 52 | yInfoVerbose("|> Local port prefix: " + local_); 53 | if (local_ == "") 54 | { 55 | yErrorVerbose("Invalid local port prefix name."); 56 | return false; 57 | } 58 | local_ += "/cmd:o"; 59 | yInfoVerbose("|> Local port command name: " + local_); 60 | 61 | remote_ = config.check("remote", Value("")).asString(); 62 | yInfoVerbose("|> Remote port name: " + remote_); 63 | if (remote_ == "") 64 | { 65 | yErrorVerbose("Invalid remote port prefix name."); 66 | return false; 67 | } 68 | remote_ += "/cmd:i"; 69 | yInfoVerbose("|> Remote port command name: " + remote_); 70 | 71 | if (!port_rpc_command_.open(local_)) 72 | { 73 | yErrorVerbose("Could not open " + local_ + " port."); 74 | return false; 75 | } 76 | 77 | if (!Network::connect(local_, remote_, "tcp", verbosity_)) 78 | { 79 | yErrorVerbose("Could not connect to " + local_ + " remote port."); 80 | return false; 81 | } 82 | 83 | if (!visualservoing_control.yarp().attachAsClient(port_rpc_command_)) 84 | { 85 | yErrorVerbose("Cannot attach the RPC client command port."); 86 | return false; 87 | } 88 | 89 | yInfoVerbose("*** VisualServoingClient configured! ***"); 90 | 91 | return true; 92 | } 93 | 94 | 95 | bool VisualServoingClient::close() 96 | { 97 | yInfoVerbose("*** Interrupting VisualServoingClient ***"); 98 | 99 | yInfoVerbose("Interrupting ports."); 100 | port_rpc_command_.interrupt(); 101 | 102 | yInfoVerbose("*** Interrupting VisualServoingClient done! ***"); 103 | 104 | 105 | yInfoVerbose("*** Closing VisualServoingClient ***"); 106 | 107 | yInfoVerbose("Closing ports."); 108 | port_rpc_command_.close(); 109 | 110 | yInfoVerbose("*** Closing VisualServoingClient done! ***"); 111 | 112 | 113 | return true; 114 | } 115 | 116 | 117 | /* IVisualServoing overrides */ 118 | bool VisualServoingClient::initFacilities(const bool use_direct_kin) 119 | { 120 | return visualservoing_control.init_facilities(use_direct_kin); 121 | } 122 | 123 | 124 | bool VisualServoingClient::resetFacilities() 125 | { 126 | return visualservoing_control.reset_facilities(); 127 | } 128 | 129 | 130 | bool VisualServoingClient::stopFacilities() 131 | { 132 | return visualservoing_control.stop_facilities(); 133 | } 134 | 135 | 136 | bool VisualServoingClient::goToGoal(const Vector& vec_x, const Vector& vec_o) 137 | { 138 | if (vec_x.size() != 3 || vec_o.size() != 4) 139 | return false; 140 | 141 | std::vector std_vec_x(vec_x.data(), vec_x.data() + vec_x.size()); 142 | std::vector std_vec_o(vec_o.data(), vec_o.data() + vec_o.size()); 143 | 144 | return visualservoing_control.go_to_pose_goal(std_vec_x, std_vec_o); 145 | } 146 | 147 | 148 | bool VisualServoingClient::goToGoal(const std::vector& vec_px_l, const std::vector& vec_px_r) 149 | { 150 | if (vec_px_l.size() != vec_px_r.size()) 151 | return false; 152 | 153 | const size_t num_points = vec_px_l.size(); 154 | 155 | std::vector> std_vec_px_l(num_points); 156 | std::vector> std_vec_px_r(num_points); 157 | 158 | for (size_t i = 0; i < num_points; ++i) 159 | { 160 | const Vector& yv_l = vec_px_l[i]; 161 | std::vector& v_l = std_vec_px_l[i]; 162 | 163 | const Vector& yv_r = vec_px_r[i]; 164 | std::vector& v_r = std_vec_px_r[i]; 165 | 166 | v_l = std::vector(yv_l.data(), yv_l.data() + yv_l.size()); 167 | v_r = std::vector(yv_r.data(), yv_r.data() + yv_r.size()); 168 | } 169 | 170 | return visualservoing_control.go_to_px_goal(std_vec_px_l, std_vec_px_r); 171 | } 172 | 173 | 174 | bool VisualServoingClient::setModality(const std::string& mode) 175 | { 176 | return visualservoing_control.set_modality(mode); 177 | } 178 | 179 | 180 | bool VisualServoingClient::setVisualServoControl(const std::string& control) 181 | { 182 | return visualservoing_control.set_visual_servo_control(control); 183 | } 184 | 185 | 186 | bool VisualServoingClient::setControlPoint(const yarp::os::ConstString& point) 187 | { 188 | yWarningVerbose("*** Service setControlPoint is unimplemented. ***"); 189 | 190 | return false; 191 | } 192 | 193 | 194 | bool VisualServoingClient::getVisualServoingInfo(yarp::os::Bottle& info) 195 | { 196 | yWarningVerbose("*** Service getVisualServoingInfo is unimplemented. ***"); 197 | 198 | return false; 199 | } 200 | 201 | 202 | bool VisualServoingClient::setGoToGoalTolerance(const double tol) 203 | { 204 | return visualservoing_control.set_go_to_goal_tolerance(tol); 205 | } 206 | 207 | 208 | bool VisualServoingClient::checkVisualServoingController() 209 | { 210 | return visualservoing_control.check_visual_servoing_controller(); 211 | } 212 | 213 | 214 | bool VisualServoingClient::waitVisualServoingDone(const double period, const double timeout) 215 | { 216 | return visualservoing_control.wait_visual_servoing_done(period, timeout); 217 | } 218 | 219 | 220 | bool VisualServoingClient::stopController() 221 | { 222 | return visualservoing_control.stop_controller(); 223 | } 224 | 225 | 226 | bool VisualServoingClient::setTranslationGain(const double K_x_1, const double K_x_2) 227 | { 228 | return visualservoing_control.set_translation_gain(K_x_1, K_x_2); 229 | } 230 | 231 | 232 | bool VisualServoingClient::setMaxTranslationVelocity(const double max_x_dot) 233 | { 234 | return visualservoing_control.set_max_translation_velocity(max_x_dot); 235 | } 236 | 237 | 238 | bool VisualServoingClient::setTranslationGainSwitchTolerance(const double K_x_tol) 239 | { 240 | return visualservoing_control.set_translation_gain_switch_tolerance(K_x_tol); 241 | } 242 | 243 | 244 | bool VisualServoingClient::setOrientationGain(const double K_o_1, const double K_o_2) 245 | { 246 | return visualservoing_control.set_orientation_gain(K_o_1, K_o_2); 247 | } 248 | 249 | 250 | bool VisualServoingClient::setMaxOrientationVelocity(const double max_o_dot) 251 | { 252 | return visualservoing_control.set_max_orientation_velocity(max_o_dot); 253 | } 254 | 255 | 256 | bool VisualServoingClient::setOrientationGainSwitchTolerance(const double K_o_tol) 257 | { 258 | return visualservoing_control.set_orientation_gain_switch_tolerance(K_o_tol); 259 | } 260 | 261 | 262 | std::vector VisualServoingClient::get3DGoalPositionsFrom3DPose(const Vector& x, const Vector& o) 263 | { 264 | std::vector std_x(x.data(), x.data() + x.size()); 265 | std::vector std_o(o.data(), o.data() + o.size()); 266 | 267 | std::vector> std_vec_goal_points = visualservoing_control.get_3D_goal_positions_from_3D_pose(std_x, std_o); 268 | 269 | size_t num_points = std_vec_goal_points.size(); 270 | std::vector vec_goal_points(num_points); 271 | for (size_t i = 0; i < num_points; ++i) 272 | vec_goal_points[i] = Vector(std_vec_goal_points[i].size(), std_vec_goal_points[i].data()); 273 | 274 | return vec_goal_points; 275 | } 276 | 277 | 278 | std::vector VisualServoingClient::getGoalPixelsFrom3DPose(const Vector& x, const Vector& o, const CamSel& cam) 279 | { 280 | std::vector std_x(x.data(), x.data() + x.size()); 281 | std::vector std_o(o.data(), o.data() + o.size()); 282 | 283 | std::vector> std_vec_goal_points = visualservoing_control.get_goal_pixels_from_3D_pose(std_x, std_o, (cam == CamSel::left ? "left" : "right")); 284 | 285 | size_t num_points = std_vec_goal_points.size(); 286 | std::vector vec_goal_points(num_points); 287 | for (size_t i = 0; i < num_points; ++i) 288 | vec_goal_points[i] = Vector(std_vec_goal_points[i].size(), std_vec_goal_points[i].data()); 289 | 290 | return vec_goal_points; 291 | } 292 | 293 | 294 | bool VisualServoingClient::storedInit(const std::string& label) 295 | { 296 | return visualservoing_control.stored_init(label); 297 | } 298 | 299 | 300 | bool VisualServoingClient::storedGoToGoal(const std::string& label) 301 | { 302 | return visualservoing_control.stored_go_to_goal(label); 303 | } 304 | 305 | 306 | bool VisualServoingClient::goToSFMGoal() 307 | { 308 | return visualservoing_control.get_goal_from_sfm(); 309 | } 310 | -------------------------------------------------------------------------------- /src/visualservoing/visualservoingcommon/app/BackgroundApps-SIM.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | /yarprun 8 | /vmdeb 9 | 10 | 11 | 12 | 13 | BackgroundApps-SIM 14 | Background applications for iCub_SIM-based simulations 15 | 0.5.0.0 16 | 17 | 18 | Claudio Fantacci 19 | 20 | 21 | 22 | 23 | 24 | yarplogger 25 | yarprun 26 | --start 27 | 28 | 29 | 30 | iCub_SIM 31 | vmdeb 32 | 33 | /yarplogger 34 | 35 | 36 | 37 | 38 | yarprobotinterface 39 | --context simCartesianControl 40 | yarprun 41 | 42 | /icubSim/right_arm/state:o 43 | 44 | 45 | 46 | 47 | iKinCartesianSolver 48 | --context simCartesianControl --part right_arm 49 | yarprun 50 | 51 | /icubSim/right_arm/state:o 52 | 53 | 54 | 55 | 56 | iKinGazeCtrl 57 | --from configSim.ini 58 | yarprun 59 | 60 | /icubSim/head/state:o 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /src/visualservoing/visualservoingcommon/app/visualservoing-SIM.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | visualservoing-SIM 7 | This app allows iCub in iCub_SIM to perform visual servoing tasks. 8 | 0.5.0.0 9 | 10 | 11 | Claudio Fantacci 12 | 13 | 14 | 15 | 16 | 17 | yarpdev 18 | --device visualservoingserver --verbosity true --simulate true --robot icubSim 19 | yarprun 20 | 21 | 22 | 23 | yarpview 24 | --name /view/cam_left --out /view/cam_left/click:o --x 20 --y 370 --RefreshTime 30 25 | yarprun 26 | 27 | 28 | 29 | yarpview 30 | --name /view/cam_right --out /view/cam_right/click:o --x 340 --y 370 --RefreshTime 30 31 | yarprun 32 | 33 | 34 | 35 | yarpview 36 | --name /view/cam_left/ee --x 20 --y 740 --RefreshTime 30 37 | yarprun 38 | 39 | 40 | 41 | yarpview 42 | --name /view/cam_right/ee --x 340 --y 740 --RefreshTime 30 43 | yarprun 44 | 45 | 46 | 47 | 48 | 49 | /icubSim/cam/left 50 | /view/cam_left 51 | mcast 52 | 53 | 54 | 55 | /icubSim/cam/left 56 | /visualservoing/cam_left/img:i 57 | mcast 58 | 59 | 60 | 61 | /visualservoing/cam_left/img:o 62 | /view/cam_left/ee 63 | tcp 64 | 65 | 66 | 67 | /view/cam_left/click:o 68 | /visualservoing/cam_left/click:i 69 | tcp 70 | 71 | 72 | 73 | /icubSim/cam/right 74 | /view/cam_right 75 | mcast 76 | 77 | 78 | 79 | /icubSim/cam/right 80 | /visualservoing/cam_right/img:i 81 | mcast 82 | 83 | 84 | 85 | /visualservoing/cam_right/img:o 86 | /view/cam_right/ee 87 | tcp 88 | 89 | 90 | 91 | /view/cam_right/click:o 92 | /visualservoing/cam_right/click:i 93 | tcp 94 | 95 | 96 | 97 | -------------------------------------------------------------------------------- /src/visualservoing/visualservoingcommon/app/visualservoing-click.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | visualservoing-click 7 | This app allows iCub to perform visual servoing tasks. 8 | 0.5.0.0 9 | 10 | 11 | Claudio Fantacci 12 | 13 | 14 | 15 | 16 | 17 | yarpdev 18 | --device visualservoingserver --verbosity true --robot icub 19 | localhost 20 | 21 | 22 | 23 | yarpview 24 | --name /view/cam_left --out /view/cam_left/click:o --x 20 --y 370 --RefreshTime 30 25 | localhost 26 | 27 | 28 | 29 | yarpview 30 | --name /view/cam_right --out /view/cam_right/click:o --x 340 --y 370 --RefreshTime 30 31 | localhost 32 | 33 | 34 | 35 | yarpview 36 | --name /view/cam_left/ee --x 20 --y 740 --RefreshTime 30 37 | localhost 38 | 39 | 40 | 41 | yarpview 42 | --name /view/cam_right/ee --x 1620 --y 0 --RefreshTime 30 --compact 43 | icub22 44 | 45 | 46 | 47 | 48 | 49 | /icub/camcalib/left/out 50 | /view/cam_left 51 | mcast 52 | 53 | 54 | 55 | /icub/camcalib/left/out 56 | /visualservoing/cam_left/img:i 57 | mcast 58 | 59 | 60 | 61 | /visualservoing/cam_left/img:o 62 | /view/cam_left/ee 63 | tcp 64 | 65 | 66 | 67 | /view/cam_left/click:o 68 | /visualservoing/cam_left/click:i 69 | tcp 70 | 71 | 72 | 73 | /view/cam_right/click:o 74 | /visualservoing/cam_right/click:i 75 | tcp 76 | 77 | 78 | 79 | /icub/camcalib/right/out 80 | /view/cam_right 81 | mcast 82 | 83 | 84 | 85 | /icub/camcalib/right/out 86 | /visualservoing/cam_right/img:i 87 | mcast 88 | 89 | 90 | 91 | /visualservoing/cam_right/img:o 92 | /view/cam_right/ee 93 | tcp 94 | 95 | 96 | 97 | -------------------------------------------------------------------------------- /src/visualservoing/visualservoingcommon/app/visualservoing-datadump.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | visualservoing-datadump 7 | This app allows iCub to perform visual servoing tasks while saving data. 8 | 0.5.0.0 9 | 10 | 11 | Claudio Fantacci 12 | 13 | 14 | 15 | 16 | 17 | yarpdev 18 | --device visualservoingserver --verbosity true --robot icub 19 | localhost 20 | 21 | 22 | 23 | yarpview 24 | --name /view/cam_left --out /view/cam_left/click:o --x 20 --y 370 --RefreshTime 30 25 | localhost 26 | 27 | 28 | 29 | yarpview 30 | --name /view/cam_right --out /view/cam_right/click:o --x 340 --y 370 --RefreshTime 30 31 | localhost 32 | 33 | 34 | 35 | yarpview 36 | --name /view/cam_left/ee --x 20 --y 740 --RefreshTime 30 37 | localhost 38 | 39 | 40 | 41 | yarpview 42 | --name /view/cam_right/ee --x 340 --y 740 --RefreshTime 30 43 | localhost 44 | 45 | 46 | 47 | 48 | 49 | yarpdatadumper 50 | localhost 51 | --name /datadumper/cam_left/ctrl --txTime --rxTime --type bottle 52 | 53 | 54 | 55 | yarpdatadumper 56 | localhost 57 | --name /datadumper/cam_right/ctrl --txTime --rxTime --type bottle 58 | 59 | 60 | 61 | yarpdatadumper 62 | localhost 63 | --name /datadumper/cam_left/kin --txTime --rxTime --type bottle 64 | 65 | 66 | 67 | yarpdatadumper 68 | localhost 69 | --name /datadumper/cam_right/kin --txTime --rxTime --type bottle 70 | 71 | 72 | 73 | yarpdatadumper 74 | localhost 75 | --name /datadumper/cam_left/goal --txTime --rxTime --type bottle 76 | 77 | 78 | 79 | yarpdatadumper 80 | localhost 81 | --name /datadumper/cam_right/goal --txTime --rxTime --type bottle 82 | 83 | 84 | 85 | yarpdatadumper 86 | localhost 87 | --name /datadumper/pose/ctrl --txTime --rxTime --type bottle 88 | 89 | 90 | 91 | yarpdatadumper 92 | localhost 93 | --name /datadumper/pose/goal --txTime --rxTime --type bottle 94 | 95 | 96 | 97 | yarpdatadumper 98 | localhost 99 | --name /datadumper/pose/kin --txTime --rxTime --type bottle 100 | 101 | 102 | 103 | 104 | 105 | /icub/camcalib/left/out 106 | /view/cam_left 107 | mcast 108 | 109 | 110 | 111 | /icub/camcalib/left/out 112 | /visualservoing/cam_left/img:i 113 | mcast 114 | 115 | 116 | 117 | /visualservoing/cam_left/img:o 118 | /view/cam_left/ee 119 | tcp 120 | 121 | 122 | 123 | /view/cam_left/click:o 124 | /visualservoing/cam_left/click:i 125 | tcp 126 | 127 | 128 | 129 | /view/cam_right/click:o 130 | /visualservoing/cam_right/click:i 131 | tcp 132 | 133 | 134 | 135 | /icub/camcalib/right/out 136 | /view/cam_right 137 | mcast 138 | 139 | 140 | 141 | /icub/camcalib/right/out 142 | /visualservoing/cam_right/img:i 143 | mcast 144 | 145 | 146 | 147 | /visualservoing/cam_right/img:o 148 | /view/cam_right/ee 149 | tcp 150 | 151 | 152 | 153 | 154 | 155 | /visualservoing/cam_left/ctrl:o 156 | /datadumper/cam_left/ctrl 157 | tcp 158 | 159 | 160 | 161 | /visualservoing/cam_right/ctrl:o 162 | /datadumper/cam_right/ctrl 163 | tcp 164 | 165 | 166 | 167 | /visualservoing/cam_left/kin:o 168 | /datadumper/cam_left/kin 169 | tcp 170 | 171 | 172 | 173 | /visualservoing/cam_right/kin:o 174 | /datadumper/cam_right/kin 175 | tcp 176 | 177 | 178 | 179 | /visualservoing/cam_left/goal:o 180 | /datadumper/cam_left/goal 181 | tcp 182 | 183 | 184 | 185 | /visualservoing/cam_right/goal:o 186 | /datadumper/cam_right/goal 187 | tcp 188 | 189 | 190 | 191 | /visualservoing/pose/ctrl:o 192 | /datadumper/pose/ctrl 193 | tcp 194 | 195 | 196 | 197 | /visualservoing/pose/kin:o 198 | /datadumper/pose/kin 199 | tcp 200 | 201 | 202 | 203 | /visualservoing/pose/goal:o 204 | /datadumper/pose/goal 205 | tcp 206 | 207 | 208 | 209 | -------------------------------------------------------------------------------- /src/visualservoing/visualservoingcommon/app/visualservoing.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | visualservoing 7 | This app allows iCub to perform visual servoing tasks. 8 | 0.5.0.0 9 | 10 | 11 | Claudio Fantacci 12 | 13 | 14 | 15 | 16 | 17 | yarpdev 18 | --device visualservoingserver --verbosity true --robot icub 19 | localhost 20 | 21 | 22 | 23 | yarpview 24 | --name /view/cam_left/ee --x 20 --y 740 --RefreshTime 30 25 | localhost 26 | 27 | 28 | 29 | yarpview 30 | --name /view/cam_right/ee --x 1620 --y 0 --RefreshTime 30 --compact 31 | icub22 32 | 33 | 34 | 35 | 36 | 37 | /icub/camcalib/left/out 38 | /view/cam_left 39 | mcast 40 | 41 | 42 | 43 | /icub/camcalib/left/out 44 | /visualservoing/cam_left/img:i 45 | mcast 46 | 47 | 48 | 49 | /visualservoing/cam_left/img:o 50 | /view/cam_left/ee 51 | tcp 52 | 53 | 54 | 55 | /icub/camcalib/right/out 56 | /view/cam_right 57 | mcast 58 | 59 | 60 | 61 | /icub/camcalib/right/out 62 | /visualservoing/cam_right/img:i 63 | mcast 64 | 65 | 66 | 67 | /visualservoing/cam_right/img:o 68 | /view/cam_right/ee 69 | tcp 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /src/visualservoing/visualservoingcommon/include/VisualServoingCommon.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef VISUALSERVOINGCOMMON_H 9 | #define VISUALSERVOINGCOMMON_H 10 | 11 | #endif /* VISUALSERVOINGCOMMON_H */ 12 | -------------------------------------------------------------------------------- /src/visualservoing/visualservoingserver-app/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # BSD 3-Clause license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | set(APP_TARGET_NAME visualservoingserver_app) 11 | 12 | find_package(YARP REQUIRED) 13 | 14 | include(YarpInstallationHelpers) 15 | 16 | LIST(APPEND CMAKE_MODULE_PATH ${YARP_MODULE_PATH}) 17 | 18 | include(YarpInstallationHelpers) 19 | 20 | # Application source and header files 21 | set(${APP_TARGET_NAME}_SRC 22 | src/main.cpp) 23 | 24 | # Application target calls 25 | add_executable(${APP_TARGET_NAME} ${${APP_TARGET_NAME}_SRC}) 26 | 27 | target_include_directories(${APP_TARGET_NAME} PRIVATE ${YARP_INCLUDE_DIRS}) 28 | 29 | target_link_libraries(${APP_TARGET_NAME} PRIVATE ${YARP_LIBRARIES} 30 | visualservoingplugin) 31 | 32 | install(TARGETS ${APP_TARGET_NAME} DESTINATION bin) 33 | -------------------------------------------------------------------------------- /src/visualservoing/visualservoingserver-app/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | using namespace yarp::os; 16 | using namespace yarp::sig; 17 | using namespace yarp::dev; 18 | using namespace yarp::math; 19 | 20 | YARP_DECLARE_PLUGINS(visualservoingplugin); 21 | 22 | int main(int argc, char **argv) 23 | { 24 | Network yarp; 25 | if (!yarp.checkNetwork(3.0)) 26 | { 27 | yError("YARP seems unavailable!"); 28 | return EXIT_FAILURE; 29 | } 30 | 31 | YARP_REGISTER_PLUGINS(visualservoingplugin); 32 | 33 | Property prop_server_vs; 34 | prop_server_vs.put("device", "visualservoingserver"); 35 | prop_server_vs.put("verbosity", true); 36 | prop_server_vs.put("simulate", true); 37 | prop_server_vs.put("robot", "icubSim"); 38 | bool use_fwd_kin = false; 39 | 40 | PolyDriver drv_server_vs(prop_server_vs); 41 | if (!drv_server_vs.isValid()) 42 | { 43 | yError("Could not run VisualServoingServer!"); 44 | return EXIT_FAILURE; 45 | } 46 | 47 | IVisualServoing *visual_servoing; 48 | drv_server_vs.view(visual_servoing); 49 | if (visual_servoing == YARP_NULLPTR) 50 | { 51 | yError("Could not get interfacate to VisualServoingServer!"); 52 | return EXIT_FAILURE; 53 | } 54 | 55 | /* Set visual servoing control*/ 56 | visual_servoing->setVisualServoControl("decoupled"); 57 | 58 | /* Stored set-up: t170904 */ 59 | visual_servoing->storedInit("t170904"); 60 | 61 | visual_servoing->initFacilities(use_fwd_kin); 62 | visual_servoing->storedGoToGoal("t170904"); 63 | 64 | visual_servoing->checkVisualServoingController(); 65 | visual_servoing->waitVisualServoingDone(); 66 | 67 | visual_servoing->stopFacilities(); 68 | 69 | /* Stored set-up: t170427 */ 70 | visual_servoing->storedInit("t170427"); 71 | 72 | visual_servoing->initFacilities(use_fwd_kin); 73 | visual_servoing->storedGoToGoal("t170427"); 74 | 75 | visual_servoing->checkVisualServoingController(); 76 | visual_servoing->waitVisualServoingDone(); 77 | 78 | visual_servoing->stopFacilities(); 79 | 80 | return EXIT_SUCCESS; 81 | } 82 | -------------------------------------------------------------------------------- /src/visualservoing/visualservoingserver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # BSD 3-Clause license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | set(PLUGIN_TARGET_NAME visualservoingserver) 11 | string(TOUPPER ${PLUGIN_NAME} UC_PLUGIN_NAME) 12 | 13 | yarp_prepare_plugin(${PLUGIN_TARGET_NAME} CATEGORY device 14 | TYPE VisualServoingServer 15 | INCLUDE VisualServoingServer.h 16 | DEFAULT ON 17 | ADVANCED 18 | INTERNAL) 19 | 20 | find_package(ICUB REQUIRED) 21 | find_package(OpenCV REQUIRED) 22 | 23 | LIST(APPEND CMAKE_MODULE_PATH ${YARP_MODULE_PATH} 24 | ${ICUB_MODULE_PATH}) 25 | 26 | include(YarpInstallationHelpers) 27 | 28 | # Plugin source and header files 29 | set(${PLUGIN_TARGET_NAME}_HDR 30 | ../visualservoingcommon/include/VisualServoingCommon.h 31 | include/VisualServoingServer.h) 32 | 33 | set(${PLUGIN_TARGET_NAME}_SRC 34 | src/VisualServoingServer.cpp) 35 | 36 | set(${PLUGIN_TARGET_NAME}_CONF 37 | conf/visualservoingserver.ini) 38 | 39 | set(${PLUGIN_TARGET_NAME}_THRIFT_HDR 40 | thrift/visualservoing.thrift) 41 | 42 | # Plugin target calls 43 | yarp_add_idl(${PLUGIN_TARGET_NAME}_THRIFT_SRC ${${PLUGIN_TARGET_NAME}_THRIFT_HDR}) 44 | 45 | yarp_add_plugin(${PLUGIN_TARGET_NAME} ${${PLUGIN_TARGET_NAME}_HDR} ${${PLUGIN_TARGET_NAME}_SRC} 46 | ${${PLUGIN_TARGET_NAME}_THRIFT_SRC}) 47 | 48 | target_include_directories(${PLUGIN_TARGET_NAME} PRIVATE "$" 49 | ${ICUB_INCLUDE_DIRS} 50 | ${YARP_INCLUDE_DIRS}) 51 | 52 | target_link_libraries(${PLUGIN_TARGET_NAME} PRIVATE ctrlLib 53 | ${ICUB_LIBRARIES} 54 | iKin 55 | ${OpenCV_LIBS} 56 | ${YARP_LIBRARIES}) 57 | 58 | yarp_install(TARGETS ${PLUGIN_TARGET_NAME} 59 | COMPONENT plugin 60 | LIBRARY DESTINATION ${ICUBCONTRIB_DYNAMIC_PLUGINS_INSTALL_DIR}) 61 | 62 | yarp_install(FILES ${${PLUGIN_TARGET_NAME}_CONF} 63 | COMPONENT plugin 64 | DESTINATION ${ICUBCONTRIB_PLUGIN_MANIFESTS_INSTALL_DIR}) 65 | -------------------------------------------------------------------------------- /src/visualservoing/visualservoingserver/conf/visualservoingserver.ini: -------------------------------------------------------------------------------- 1 | [plugin visualservoingserver] 2 | type device 3 | name visualservoingserver 4 | library visualservoingserver 5 | -------------------------------------------------------------------------------- /src/visualservoing/visualservoingserver/include/VisualServoingServer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef VISUALSERVOINGSERVER_H 9 | #define VISUALSERVOINGSERVER_H 10 | 11 | #include "thrift/VisualServoingIDL.h" 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | 39 | class VisualServoingServer : public yarp::dev::DeviceDriver, 40 | public yarp::dev::IVisualServoing, 41 | protected yarp::os::Thread, 42 | public VisualServoingIDL 43 | { 44 | public: 45 | /* Ctors and Dtors */ 46 | VisualServoingServer(); 47 | 48 | ~VisualServoingServer(); 49 | 50 | 51 | /* DeviceDriver overrides */ 52 | bool open(yarp::os::Searchable &config) override; 53 | 54 | bool close() override; 55 | 56 | 57 | /* IVisualServoing overrides */ 58 | bool initFacilities(const bool use_direct_kin) override; 59 | 60 | bool resetFacilities() override; 61 | 62 | bool stopFacilities() override; 63 | 64 | bool goToGoal(const yarp::sig::Vector& vec_x, const yarp::sig::Vector& vec_o) override; 65 | 66 | bool goToGoal(const std::vector& vec_px_l, const std::vector& vec_px_r) override; 67 | 68 | bool setModality(const std::string& mode) override; 69 | 70 | bool setVisualServoControl(const std::string& control) override; 71 | 72 | bool setControlPoint(const yarp::os::ConstString& point) override; 73 | 74 | bool getVisualServoingInfo(yarp::os::Bottle& info) override; 75 | 76 | bool setGoToGoalTolerance(const double tol = 15.0) override; 77 | 78 | bool checkVisualServoingController() override; 79 | 80 | bool waitVisualServoingDone(const double period = 0.1, const double timeout = 0.0) override; 81 | 82 | bool stopController() override; 83 | 84 | bool setTranslationGain(const double K_x_1 = 1.0, const double K_x_2 = 0.25) override; 85 | 86 | bool setMaxTranslationVelocity(const double max_x_dot) override; 87 | 88 | bool setTranslationGainSwitchTolerance(const double K_x_tol = 30.0) override; 89 | 90 | bool setOrientationGain(const double K_x_1 = 1.5, const double K_x_2 = 0.375) override; 91 | 92 | bool setMaxOrientationVelocity(const double max_o_dot) override; 93 | 94 | bool setOrientationGainSwitchTolerance(const double K_o_tol = 30.0) override; 95 | 96 | std::vector get3DGoalPositionsFrom3DPose(const yarp::sig::Vector& x, const yarp::sig::Vector& o) override; 97 | 98 | std::vector getGoalPixelsFrom3DPose(const yarp::sig::Vector& x, const yarp::sig::Vector& o, const CamSel& cam) override; 99 | 100 | 101 | /* TO BE DEPRECATED */ 102 | bool storedInit(const std::string& label) override; 103 | 104 | bool storedGoToGoal(const std::string& label) override; 105 | 106 | bool goToSFMGoal() override; 107 | 108 | protected: 109 | /* Thread overrides */ 110 | void beforeStart() override; 111 | 112 | bool threadInit() override; 113 | 114 | void run() override; 115 | 116 | void afterStart(bool success) override; 117 | 118 | void onStop() override; 119 | 120 | void threadRelease() override; 121 | 122 | 123 | /* VisualServoingIDL overrides */ 124 | bool init_facilities(const bool use_direct_kin) override; 125 | 126 | bool reset_facilities() override; 127 | 128 | bool stop_facilities() override; 129 | 130 | bool go_to_px_goal(const std::vector>& vec_px_l, const std::vector>& vec_px_r) override; 131 | 132 | bool go_to_pose_goal(const std::vector& vec_x, const std::vector& vec_o) override; 133 | 134 | bool set_modality(const std::string& mode) override; 135 | 136 | bool set_visual_servo_control(const std::string& control) override; 137 | 138 | bool set_control_point(const std::string& point) override; 139 | 140 | std::vector get_visual_servoing_info() override; 141 | 142 | bool set_go_to_goal_tolerance(const double tol) override; 143 | 144 | bool check_visual_servoing_controller() override; 145 | 146 | bool wait_visual_servoing_done(const double period, const double timeout) override; 147 | 148 | bool stop_controller() override; 149 | 150 | bool set_translation_gain(const double K_x_1, const double K_x_2) override; 151 | 152 | bool set_max_translation_velocity(const double max_x_dot) override; 153 | 154 | bool set_translation_gain_switch_tolerance(const double K_x_tol) override; 155 | 156 | bool set_orientation_gain(const double K_o_1, const double K_o_2) override; 157 | 158 | bool set_max_orientation_velocity(const double max_o_dot) override; 159 | 160 | bool set_orientation_gain_switch_tolerance(const double K_o_tol) override; 161 | 162 | std::vector> get_3D_goal_positions_from_3D_pose(const std::vector& x, const std::vector& o) override; 163 | 164 | std::vector> get_goal_pixels_from_3D_pose(const std::vector & x, const std::vector & o, const std::string& cam) override; 165 | 166 | bool quit() override; 167 | 168 | /* TO BE DEPRECATED */ 169 | bool stored_init(const std::string& label) override; 170 | 171 | bool stored_go_to_goal(const std::string& label) override; 172 | 173 | bool get_goal_from_sfm() override; 174 | 175 | 176 | /* Enum helpers */ 177 | enum class VisualServoControl { decoupled, robust, cartesian }; 178 | 179 | enum class PixelControlMode { all, x, o }; 180 | 181 | enum class OperatingMode { position, orientation, pose }; 182 | 183 | private: 184 | bool verbosity_ = false; 185 | bool sim_ = false; 186 | yarp::os::ConstString robot_name_ = "icub"; 187 | 188 | VisualServoControl vs_control_ = VisualServoControl::decoupled; 189 | OperatingMode op_mode_ = OperatingMode::pose; 190 | 191 | yarp::dev::PolyDriver rightarm_cartesian_driver_; 192 | yarp::dev::ICartesianControl* itf_rightarm_cart_; 193 | 194 | yarp::dev::PolyDriver gaze_driver_; 195 | yarp::dev::IGazeControl* itf_gaze_; 196 | 197 | bool vs_control_running_ = false; 198 | bool vs_goal_reached_ = false; 199 | const double Ts_ = 0.1; /* [s] */ 200 | std::array K_x_ = {{0.5, 0.25}}; 201 | std::array K_o_ = {{3.5, 0.5}}; 202 | double max_x_dot_ = 0.035; /* [m/s] */ 203 | double max_o_dot_ = 10.0 * M_PI / 180.0; /* [rad/s] */ 204 | double K_x_tol_ = 10.0; /* [pixel] */ 205 | double K_o_tol_ = 10.0; /* [pixel] */ 206 | double K_position_tol_ = 0.03; /* [m] */ 207 | double K_orientation_tol_ = 9.0 * M_PI / 180.0; /* [rad] */ 208 | double K_angle_tol_ = 3.0 * M_PI / 180.0; /* [rad] */ 209 | bool K_x_hysteresis_ = false; 210 | bool K_o_hysteresis_ = false; 211 | bool K_pose_hysteresis_ = false; 212 | double tol_px_ = 1.0; /* [pixel] */ 213 | double tol_position_ = 0.01; /* [m] */ 214 | double tol_orientation_ = 3.0 * M_PI / 180.0; /* [rad] */ 215 | double tol_angle_ = 1.0 * M_PI / 180.0; /* [rad] */ 216 | double traj_time_ = 1.0; /* [s] */ 217 | 218 | yarp::sig::Matrix l_proj_; 219 | yarp::sig::Matrix r_proj_; 220 | 221 | yarp::sig::Vector goal_pose_ = yarp::math::zeros(7); 222 | yarp::sig::Vector px_des_ = yarp::math::zeros(16); 223 | yarp::sig::Matrix l_H_eye_to_r_ = yarp::math::zeros(4, 4); 224 | yarp::sig::Matrix r_H_eye_to_r_ = yarp::math::zeros(4, 4); 225 | yarp::sig::Matrix l_H_r_to_cam_ = yarp::math::zeros(4, 4); 226 | yarp::sig::Matrix r_H_r_to_cam_ = yarp::math::zeros(4, 4); 227 | 228 | std::mutex mtx_px_des_; 229 | std::mutex mtx_H_eye_cam_; 230 | 231 | std::thread* thr_background_update_params_; 232 | 233 | std::vector l_px_goal_ = std::vector(4); 234 | std::vector r_px_goal_ = std::vector(4); 235 | 236 | int ctx_local_cart_; 237 | int ctx_remote_cart_; 238 | 239 | yarp::os::BufferedPort port_pose_left_in_; 240 | yarp::os::BufferedPort port_pose_right_in_; 241 | 242 | yarp::os::BufferedPort> port_image_left_in_; 243 | yarp::os::BufferedPort> port_image_left_out_; 244 | yarp::os::BufferedPort port_click_left_; 245 | 246 | yarp::os::BufferedPort> port_image_right_in_; 247 | yarp::os::BufferedPort> port_image_right_out_; 248 | yarp::os::BufferedPort port_click_right_; 249 | 250 | yarp::os::Port port_rpc_command_; 251 | yarp::os::RpcClient port_rpc_tracker_left_; 252 | yarp::os::RpcClient port_rpc_tracker_right_; 253 | 254 | /* Private class methods */ 255 | void decoupledImageBasedVisualServoControl(); 256 | 257 | void robustImageBasedVisualServoControl(); 258 | 259 | void cartesianPositionBasedVisualServoControl(); 260 | 261 | bool setRightArmCartesianController(); 262 | 263 | bool setGazeController(); 264 | 265 | bool setCommandPort(); 266 | 267 | bool setTorsoDOF(); 268 | 269 | bool unsetTorsoDOF(); 270 | 271 | std::vector getPixelsFromPose(const yarp::sig::Vector& pose, const CamSel& cam); 272 | 273 | std::vector getControlPixelsFromPose(const yarp::sig::Vector& pose, const CamSel& cam, const PixelControlMode& mode); 274 | 275 | std::vector getControlPointsFromPose(const yarp::sig::Vector& pose); 276 | 277 | yarp::sig::Vector getPixelFromPoint(const CamSel& cam, const yarp::sig::Vector& p) const; 278 | 279 | yarp::sig::Vector getControlPixelFromPoint(const CamSel& cam, const yarp::sig::Vector& p) const; 280 | 281 | void getCurrentStereoFeaturesAndJacobian(const std::vector& left_px, const std::vector& right_px, 282 | yarp::sig::Vector& features, yarp::sig::Matrix& jacobian); 283 | 284 | yarp::sig::Vector getJacobianU(const CamSel& cam, const yarp::sig::Vector& px); 285 | 286 | yarp::sig::Vector getJacobianV(const CamSel& cam, const yarp::sig::Vector& px); 287 | 288 | bool setCameraTransformations(); 289 | 290 | bool setPoseGoal(const yarp::sig::Vector& goal_x, const yarp::sig::Vector& goal_o); 291 | 292 | bool setPixelGoal(const std::vector& l_px_goal, const std::vector& r_px_goal); 293 | 294 | void backproc_UpdateVisualServoingParamters(); 295 | bool is_stopping_backproc_update_vs_params = true; 296 | 297 | yarp::sig::Vector averagePose(const yarp::sig::Vector& l_pose, const yarp::sig::Vector& r_pose) const; 298 | 299 | bool checkVisualServoingStatus(const yarp::sig::Vector& px_cur, const double tol); 300 | 301 | bool checkVisualServoingStatus(const yarp::sig::Vector& pose_cur, const double tol_position, const double tol_orientation, const double tol_angle); 302 | 303 | 304 | /* LOG */ 305 | void yInfoVerbose (const yarp::os::ConstString& str) const { if(verbosity_) yInfo() << str; }; 306 | void yWarningVerbose(const yarp::os::ConstString& str) const { if(verbosity_) yWarning() << str; }; 307 | void yErrorVerbose (const yarp::os::ConstString& str) const { if(verbosity_) yError() << str; }; 308 | 309 | yarp::sig::Vector stdVectorOfVectorsToVector(const std::vector& vectors); 310 | 311 | yarp::os::BufferedPort port_pose_px_l_; 312 | yarp::os::BufferedPort port_pose_px_r_; 313 | 314 | yarp::os::BufferedPort port_kin_px_l_; 315 | yarp::os::BufferedPort port_kin_px_r_; 316 | 317 | yarp::os::BufferedPort port_goal_px_l_; 318 | yarp::os::BufferedPort port_goal_px_r_; 319 | 320 | yarp::os::BufferedPort port_pose_avg_; 321 | yarp::os::BufferedPort port_pose_kin_; 322 | yarp::os::BufferedPort port_pose_goal_; 323 | }; 324 | 325 | #endif /* VISUALSERVOINGSERVER_H */ 326 | --------------------------------------------------------------------------------