├── .github └── workflows │ ├── ci.yml │ └── doxygen.yml ├── .gitignore ├── CMakeLists.txt ├── CONTRIBUTING.md ├── LICENSE ├── README.md ├── bindings ├── CMakeLists.txt ├── python │ └── CMakeLists.txt └── roboticslab_kinematics_dynamics.i ├── cmake └── find-modules │ └── FindGTestSources.cmake ├── doc ├── CMakeLists.txt ├── fig │ └── kinematics-dynamics.png ├── groups.dox ├── kinematics-dynamics-install.md ├── kinematics-dynamics.bib └── teo-post-install.md ├── examples ├── cpp │ ├── CMakeLists.txt │ ├── exampleCartesianControlClient │ │ ├── CMakeLists.txt │ │ └── exampleCartesianControlClient.cpp │ ├── exampleScrewTheoryTrajectory │ │ ├── CMakeLists.txt │ │ ├── TrajectoryThread.cpp │ │ ├── TrajectoryThread.hpp │ │ └── exampleScrewTheoryTrajectory.cpp │ └── exampleYarpTinyMath │ │ ├── CMakeLists.txt │ │ └── exampleYarpTinyMath.cpp └── python │ ├── exampleCartesianControlClient.py │ └── exampleKdlSolverFromFile.py ├── libraries ├── CMakeLists.txt ├── KdlSolverUtils │ ├── CMakeLists.txt │ ├── KdlSolverUtils.cpp │ └── KdlSolverUtils.hpp ├── KdlVectorConverterLib │ ├── CMakeLists.txt │ ├── KdlVectorConverter.cpp │ └── KdlVectorConverter.hpp ├── KinematicRepresentationLib │ ├── CMakeLists.txt │ ├── KinematicRepresentation.cpp │ └── KinematicRepresentation.hpp ├── ScrewTheoryLib │ ├── CMakeLists.txt │ ├── ConfigurationSelector.cpp │ ├── ConfigurationSelector.hpp │ ├── ConfigurationSelectorHumanoidGait.cpp │ ├── ConfigurationSelectorLeastOverallAngularDisplacement.cpp │ ├── LogComponent.cpp │ ├── LogComponent.hpp │ ├── MatrixExponential.cpp │ ├── MatrixExponential.hpp │ ├── PadenKahanSubproblems.cpp │ ├── PardosGotorSubproblems.cpp │ ├── ProductOfExponentials.cpp │ ├── ProductOfExponentials.hpp │ ├── ScrewTheoryIkProblem.cpp │ ├── ScrewTheoryIkProblem.hpp │ ├── ScrewTheoryIkProblemBuilder.cpp │ ├── ScrewTheoryIkSubproblems.hpp │ ├── ScrewTheoryTools.cpp │ └── ScrewTheoryTools.hpp ├── YarpPlugins │ ├── AsibotSolver │ │ ├── AsibotConfiguration.cpp │ │ ├── AsibotConfiguration.hpp │ │ ├── AsibotConfigurationLeastOverallAngularDisplacement.cpp │ │ ├── AsibotSolver.cpp │ │ ├── AsibotSolver.hpp │ │ ├── AsibotSolver_ParamsParser.cpp │ │ ├── AsibotSolver_ParamsParser.h │ │ ├── AsibotSolver_params.md │ │ ├── CMakeLists.txt │ │ ├── DeviceDriverImpl.cpp │ │ ├── ICartesianSolverImpl.cpp │ │ ├── LogComponent.cpp │ │ ├── LogComponent.hpp │ │ └── TrajGen.hpp │ ├── BasicCartesianControl │ │ ├── BasicCartesianControl.cpp │ │ ├── BasicCartesianControl.hpp │ │ ├── BasicCartesianControl_ParamsParser.cpp │ │ ├── BasicCartesianControl_ParamsParser.h │ │ ├── BasicCartesianControl_params.md │ │ ├── CMakeLists.txt │ │ ├── DeviceDriverImpl.cpp │ │ ├── ICartesianControlImpl.cpp │ │ ├── LogComponent.cpp │ │ ├── LogComponent.hpp │ │ └── PeriodicThreadImpl.cpp │ ├── CMakeLists.txt │ ├── CartesianControlClient │ │ ├── CMakeLists.txt │ │ ├── CartesianControlClient.hpp │ │ ├── CartesianControlClient_ParamsParser.cpp │ │ ├── CartesianControlClient_ParamsParser.h │ │ ├── CartesianControlClient_params.md │ │ ├── DeviceDriverImpl.cpp │ │ ├── FkStreamResponder.cpp │ │ ├── ICartesianControlImpl.cpp │ │ ├── LogComponent.cpp │ │ └── LogComponent.hpp │ ├── CartesianControlServer │ │ ├── CMakeLists.txt │ │ ├── CartesianControlServer.hpp │ │ ├── CartesianControlServer_ParamsParser.cpp │ │ ├── CartesianControlServer_ParamsParser.h │ │ ├── CartesianControlServer_params.md │ │ ├── DeviceDriverImpl.cpp │ │ ├── IWrapperImpl.cpp │ │ ├── LogComponent.cpp │ │ ├── LogComponent.hpp │ │ ├── PeriodicThreadImpl.cpp │ │ ├── RpcResponder.cpp │ │ └── StreamResponder.cpp │ ├── CartesianControlServerROS2 │ │ ├── CMakeLists.txt │ │ ├── CartesianControlServerROS2.cpp │ │ ├── CartesianControlServerROS2.hpp │ │ ├── CartesianControlServerROS2_ParamsParser.cpp │ │ ├── CartesianControlServerROS2_ParamsParser.h │ │ ├── CartesianControlServerROS2_params.md │ │ ├── DeviceDriverImpl.cpp │ │ ├── IWrapperImpl.cpp │ │ ├── LogComponent.cpp │ │ ├── LogComponent.hpp │ │ ├── PeriodicThreadImpl.cpp │ │ ├── README.md │ │ ├── Spinner.cpp │ │ ├── Spinner.hpp │ │ └── fig │ │ │ └── CartesianControlServer_ROS2-YARP.png │ ├── ICartesianControl.h │ ├── ICartesianSolver.h │ ├── KdlSolver │ │ ├── CMakeLists.txt │ │ ├── ChainFkSolverPos_ST.cpp │ │ ├── ChainFkSolverPos_ST.hpp │ │ ├── ChainIkSolverPos_ID.cpp │ │ ├── ChainIkSolverPos_ID.hpp │ │ ├── ChainIkSolverPos_ST.cpp │ │ ├── ChainIkSolverPos_ST.hpp │ │ ├── DeviceDriverImpl.cpp │ │ ├── ICartesianSolverImpl.cpp │ │ ├── KdlSolver.hpp │ │ ├── KdlSolver_ParamsParser.cpp │ │ ├── KdlSolver_ParamsParser.h │ │ ├── KdlSolver_params.md │ │ ├── LogComponent.cpp │ │ └── LogComponent.hpp │ └── KdlTreeSolver │ │ ├── CMakeLists.txt │ │ ├── DeviceDriverImpl.cpp │ │ ├── ICartesianSolverImpl.cpp │ │ ├── KdlTreeSolver.hpp │ │ ├── KdlTreeSolver_ParamsParser.cpp │ │ ├── KdlTreeSolver_ParamsParser.h │ │ ├── KdlTreeSolver_params.md │ │ ├── LogComponent.cpp │ │ └── LogComponent.hpp └── YarpTinyMathLib │ ├── CMakeLists.txt │ ├── YarpTinyMath.cpp │ └── YarpTinyMath.hpp ├── programs ├── CMakeLists.txt ├── ftCompensation │ ├── CMakeLists.txt │ ├── FtCompensation.cpp │ ├── FtCompensation.hpp │ └── main.cpp ├── haarDetectionController │ ├── CMakeLists.txt │ ├── GrabberResponder.cpp │ ├── GrabberResponder.hpp │ ├── HaarDetectionController.cpp │ ├── HaarDetectionController.hpp │ ├── LogComponent.cpp │ ├── LogComponent.hpp │ └── main.cpp ├── keyboardController │ ├── CMakeLists.txt │ ├── KeyboardController.cpp │ ├── KeyboardController.hpp │ ├── LinearTrajectoryThread.cpp │ ├── LinearTrajectoryThread.hpp │ ├── LogComponent.cpp │ ├── LogComponent.hpp │ └── main.cpp └── streamingDeviceController │ ├── CMakeLists.txt │ ├── CentroidTransform.cpp │ ├── CentroidTransform.hpp │ ├── LeapMotionSensorDevice.cpp │ ├── LeapMotionSensorDevice.hpp │ ├── LogComponent.cpp │ ├── LogComponent.hpp │ ├── SpnavSensorDevice.cpp │ ├── SpnavSensorDevice.hpp │ ├── StreamingDevice.cpp │ ├── StreamingDevice.hpp │ ├── StreamingDeviceController.cpp │ ├── StreamingDeviceController.hpp │ ├── WiimoteSensorDevice.cpp │ ├── WiimoteSensorDevice.hpp │ └── main.cpp ├── scripts └── python │ ├── README.md │ ├── find-reachable-cuboid.py │ ├── kdl-from-csv.py │ └── roboview-from-csv.py ├── share ├── CMakeLists.txt ├── applications │ ├── ftCompensation.xml │ └── ymanager.ini └── contexts │ ├── ftCompensation-lacquey.ini │ ├── ftCompensation-stump.ini │ ├── streamingDeviceController.ini │ ├── testAsibotSolverFromFile.ini │ └── testKdlSolverFromFile.ini └── tests ├── CMakeLists.txt ├── testAsibotSolverFromFile.cpp ├── testBasicCartesianControl.cpp ├── testKdlSolver.cpp ├── testKdlSolverFromFile.cpp ├── testKinRepresentation.cpp └── testScrewTheory.cpp /.github/workflows/doxygen.yml: -------------------------------------------------------------------------------- 1 | name: Doxygen 2 | 3 | on: 4 | push: 5 | branches: 6 | - master 7 | paths: 8 | - .github/workflows/doxygen.yml 9 | # keep in sync with doxygen_add_docs() at doc/CMakeLists.txt 10 | - 'README.md' 11 | - 'doc/**' 12 | - 'examples/**' 13 | - 'libraries/**' 14 | - 'programs/**' 15 | - 'share/**' 16 | - 'tests/**' 17 | workflow_dispatch: 18 | 19 | jobs: 20 | deploy: 21 | runs-on: ubuntu-latest 22 | steps: 23 | - uses: fkirc/skip-duplicate-actions@master 24 | with: 25 | cancel_others: 'true' 26 | - run: sudo apt-get install -qq --no-install-recommends doxygen texlive-bibtex-extra 27 | - uses: actions/checkout@v2 28 | - run: cmake -S doc -B doc/build 29 | - run: cmake --build doc/build --target dox 30 | - uses: crazy-max/ghaction-github-pages@v2 31 | with: 32 | build_dir: doc/build/html 33 | env: 34 | GITHUB_TOKEN: ${{secrets.GITHUB_TOKEN}} 35 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | *.swp 3 | *.orig 4 | *.pyc 5 | .Rhistory 6 | tags 7 | CMakeLists.txt.user 8 | **/build*/ 9 | /.settings/ 10 | /.cproject 11 | /.project 12 | /.vs/ 13 | /.vscode/ 14 | /external/ 15 | /doc/html/* 16 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.19...3.30) 2 | 3 | project(ROBOTICSLAB_KINEMATICS_DYNAMICS LANGUAGES CXX) 4 | 5 | # Let the user specify a configuration (only single-config generators). 6 | if(NOT CMAKE_CONFIGURATION_TYPES) 7 | # Possible values. 8 | set(_configurations Debug Release MinSizeRel RelWithDebInfo) 9 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS ${_configurations}) 10 | 11 | foreach(_conf ${_configurations}) 12 | set(_conf_string "${_conf_string} ${_conf}") 13 | endforeach() 14 | 15 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY HELPSTRING 16 | "Choose the type of build, options are:${_conf_string}") 17 | 18 | if(NOT CMAKE_BUILD_TYPE) 19 | # Encourage the user to specify build type. 20 | message(STATUS "Setting build type to 'Release' as none was specified.") 21 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY VALUE Release) 22 | endif() 23 | endif() 24 | 25 | # Pick up our cmake modules. 26 | list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/find-modules) 27 | 28 | # Hard dependencies. 29 | find_package(YARP 3.10 REQUIRED COMPONENTS os dev sig 30 | OPTIONAL_COMPONENTS math) 31 | 32 | # Soft dependencies. 33 | find_package(orocos_kdl 1.4 QUIET) 34 | find_package(rclcpp QUIET) 35 | find_package(std_msgs QUIET) 36 | find_package(std_srvs QUIET) 37 | find_package(geometry_msgs QUIET) 38 | find_package(GTestSources 1.8 QUIET) 39 | find_package(SWIG 3.0.12 QUIET) 40 | find_package(Doxygen QUIET) 41 | 42 | # Always build YARP devices as MODULE libraries. 43 | set(YARP_FORCE_DYNAMIC_PLUGINS TRUE CACHE INTERNAL "Force dynamic plugins") 44 | 45 | # Configure installation paths for YARP resources. 46 | yarp_configure_external_installation(roboticslab-kinematics-dynamics WITH_PLUGINS) 47 | 48 | # Standard installation directories. 49 | include(GNUInstallDirs) 50 | 51 | # Control where libraries and executables are placed during the build. 52 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_BINDIR}) 53 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}) 54 | set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}) 55 | 56 | # Create targets if specific requirements are satisfied. 57 | include(CMakeDependentOption) 58 | 59 | # Acknowledge this is a CTest-friendly project. 60 | enable_testing() 61 | 62 | # Add main contents. 63 | add_subdirectory(libraries) 64 | add_subdirectory(programs) 65 | add_subdirectory(tests) 66 | add_subdirectory(share) 67 | add_subdirectory(bindings) 68 | add_subdirectory(doc) 69 | add_subdirectory(examples/cpp) 70 | 71 | # Collect public exported dependencies. 72 | get_property(_exported_dependencies GLOBAL PROPERTY _exported_dependencies) 73 | 74 | if(DEFINED _exported_dependencies) 75 | list(REMOVE_DUPLICATES _exported_dependencies) 76 | endif() 77 | 78 | # Store the package in the user registry. 79 | set(CMAKE_EXPORT_PACKAGE_REGISTRY ON) 80 | 81 | # Create and install config files. 82 | include(InstallBasicPackageFiles) 83 | 84 | install_basic_package_files(ROBOTICSLAB_KINEMATICS_DYNAMICS 85 | VERSION 0.1.0 86 | COMPATIBILITY AnyNewerVersion 87 | NO_SET_AND_CHECK_MACRO 88 | NO_CHECK_REQUIRED_COMPONENTS_MACRO 89 | NAMESPACE ROBOTICSLAB:: 90 | DEPENDENCIES ${_exported_dependencies}) 91 | 92 | # Configure and create uninstall target. 93 | include(AddUninstallTarget) 94 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | Have a feature request or found a bug? 2 | 3 | Before opening a fresh issue, do a search on [GitHub](https://github.com/roboticslab-uc3m/kinematics-dynamics/issues?utf8=%E2%9C%93&q=is%3Aissue) and make sure it hasn't already been addressed. Here are some notes on how to detail feature requests or bug reports: 4 | * Explain, as detailed as possible, how to reproduce the issue or the specific behaviour for the feature. 5 | * Include what you expected to happen and what actually happened. 6 | * Please include information on what operating system and version you are working with. Also add any other relevant details. 7 | * Feel free to attach any other information illustrating the issue if copying and pasting text is not an option. 8 | 9 | Notes: 10 | * We follow the [Forking](https://www.atlassian.com/git/tutorials/comparing-workflows/forking-workflow/) Git workflow. 11 | -------------------------------------------------------------------------------- /bindings/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # UseSWIG generates now standard target names. 2 | cmake_policy(SET CMP0078 OLD) 3 | 4 | # UseSWIG honors SWIG_MODULE_NAME via -module flag. 5 | cmake_policy(SET CMP0086 OLD) 6 | 7 | include(CMakeDependentOption) 8 | 9 | # disable all warnings for current folder and subfolders 10 | # (unless explicitly added by the user) 11 | get_property(_USER_CMAKE_C_FLAGS CACHE CMAKE_C_FLAGS PROPERTY VALUE) 12 | get_property(_USER_CMAKE_CXX_FLAGS CACHE CMAKE_CXX_FLAGS PROPERTY VALUE) 13 | set(CMAKE_C_FLAGS "-w ${_USER_CMAKE_C_FLAGS}") 14 | set(CMAKE_CXX_FLAGS "-w ${_USER_CMAKE_CXX_FLAGS}") 15 | 16 | # This is necessary for SWIG to parse .i file 17 | get_property(_include_dirs TARGET ROBOTICSLAB::KinematicsDynamicsInterfaces PROPERTY INTERFACE_INCLUDE_DIRECTORIES) 18 | foreach(_dir IN LISTS _include_dirs) 19 | if("${_dir}" MATCHES "\\\$$") 20 | include_directories("${CMAKE_MATCH_1}") 21 | elseif("${_dir}" MATCHES "\\\$$") 22 | # Nothing to do 23 | else() 24 | include_directories(${_dir}) 25 | endif() 26 | endforeach() 27 | 28 | cmake_dependent_option(CREATE_BINDINGS "Compile optional language bindings" ON 29 | SWIG_FOUND OFF) 30 | 31 | if(CREATE_BINDINGS) 32 | # Allow passing extra flags to swig (e.g. -Wall) 33 | set(SWIG_EXTRA_FLAGS "" CACHE STRING "Extra flags passed to swig commands (e.g. -Wall)") 34 | mark_as_advanced(SWIG_EXTRA_FLAGS) 35 | 36 | set(SWIG_COMMON_FLAGS "${SWIG_EXTRA_FLAGS}") 37 | 38 | set(CREATE_BINDINGS_PYTHON FALSE CACHE BOOL "Create Python interface") 39 | 40 | # Include SWIG use file 41 | include(${SWIG_USE_FILE}) 42 | 43 | set(SWIG_RL_LIBRARIES YARP::YARP_dev 44 | ROBOTICSLAB::KinematicsDynamicsInterfaces) 45 | 46 | set(SWIG_BINDINGS_SOURCE_FILE "${CMAKE_CURRENT_SOURCE_DIR}/roboticslab_kinematics_dynamics.i") 47 | 48 | if(CREATE_BINDINGS_PYTHON) 49 | add_subdirectory(python) 50 | endif() 51 | endif() 52 | -------------------------------------------------------------------------------- /bindings/python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # this is set in the parent directory, but for some reason it does not get inherited 2 | set_source_files_properties(${SWIG_BINDINGS_SOURCE_FILE} PROPERTIES CPLUSPLUS ON) 3 | 4 | set(CMAKE_SWIG_FLAGS "-module;roboticslab_kinematics_dynamics;-threads;${SWIG_COMMON_FLAGS}") 5 | 6 | find_package(Python3 COMPONENTS Interpreter Development.Module REQUIRED) 7 | 8 | set(CMAKE_SWIG_OUTDIR "${CMAKE_BINARY_DIR}/lib/python") 9 | set(SWIG_OUTFILE_DIR "${CMAKE_CURRENT_BINARY_DIR}") 10 | 11 | swig_add_library(roboticslab_kinematics_dynamics_python 12 | LANGUAGE python 13 | SOURCES ${SWIG_BINDINGS_SOURCE_FILE}) 14 | swig_link_libraries(roboticslab_kinematics_dynamics_python Python3::Module ${SWIG_RL_LIBRARIES}) 15 | 16 | set_target_properties(${SWIG_MODULE_roboticslab_kinematics_dynamics_python_REAL_NAME} PROPERTIES OUTPUT_NAME "_roboticslab_kinematics_dynamics" 17 | LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/lib/python" 18 | # treat Python3_INCLUDE_DIRS as non-system so that it can be overriden 19 | NO_SYSTEM_FROM_IMPORTED TRUE) 20 | 21 | # INCLUDE_DIRECTORIES might have gotten polluted by 3rd-party deps, make sure 22 | # the Python3 header path has more predecence by overriding the previous value 23 | # (set by `swig_link_libraries`) and prepending it to the search path 24 | target_include_directories(${SWIG_MODULE_roboticslab_kinematics_dynamics_python_REAL_NAME} BEFORE PRIVATE ${Python3_INCLUDE_DIRS}) 25 | 26 | # installation path is determined reliably on most platforms using distutils 27 | execute_process(COMMAND ${Python3_EXECUTABLE} -c "from distutils import sysconfig; print(sysconfig.get_python_lib(plat_specific=True,standard_lib=False,prefix=''))" 28 | OUTPUT_VARIABLE Python_INSTDIR 29 | OUTPUT_STRIP_TRAILING_WHITESPACE ) 30 | 31 | set(_CMAKE_INSTALL_PYTHONDIR "${Python_INSTDIR}") 32 | set(CMAKE_INSTALL_PYTHONDIR ${_CMAKE_INSTALL_PYTHONDIR} CACHE PATH "python bindings (${_CMAKE_INSTALL_PYTHONDIR})") 33 | mark_as_advanced(CMAKE_INSTALL_PYTHONDIR) 34 | if(NOT IS_ABSOLUTE ${CMAKE_INSTALL_PYTHONDIR}) 35 | set(CMAKE_INSTALL_FULL_PYTHONDIR "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_PYTHONDIR}") 36 | else() 37 | set(CMAKE_INSTALL_FULL_PYTHONDIR "${CMAKE_INSTALL_PYTHONDIR}") 38 | endif() 39 | 40 | install(FILES ${CMAKE_BINARY_DIR}/lib/python/roboticslab_kinematics_dynamics.py 41 | DESTINATION ${CMAKE_INSTALL_PYTHONDIR}) 42 | 43 | # Update RPATH 44 | if(NOT CMAKE_SKIP_RPATH AND NOT CMAKE_SKIP_INSTALL_RPATH) 45 | file(RELATIVE_PATH _rel_path "${CMAKE_INSTALL_FULL_PYTHONDIR}" "${CMAKE_INSTALL_FULL_LIBDIR}") 46 | get_target_property(_current_rpath ${SWIG_MODULE_roboticslab_kinematics_dynamics_python_REAL_NAME} INSTALL_RPATH) 47 | if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin") 48 | list(APPEND _current_rpath "@loader_path/${_rel_path}") 49 | else() 50 | list(APPEND _current_rpath "\$ORIGIN/${_rel_path}") 51 | endif() 52 | set_target_properties(${SWIG_MODULE_roboticslab_kinematics_dynamics_python_REAL_NAME} PROPERTIES INSTALL_RPATH "${_current_rpath}") 53 | endif() 54 | 55 | install(TARGETS ${SWIG_MODULE_roboticslab_kinematics_dynamics_python_REAL_NAME} 56 | DESTINATION ${CMAKE_INSTALL_PYTHONDIR}) 57 | -------------------------------------------------------------------------------- /bindings/roboticslab_kinematics_dynamics.i: -------------------------------------------------------------------------------- 1 | ////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This is a configuration file to explain kinematics_dynamics to SWIG 4 | // 5 | // SWIG, for the most part, understands kinematics_dynamics auto-magically. 6 | // There are a few things that need to be explained: 7 | // + use of multiple inheritance 8 | // + use of names that clash with special names in Java/Python/Perl/... 9 | // + use of templates 10 | 11 | %module "roboticslab_kinematics_dynamics" 12 | 13 | %include "std_vector.i" /* Do not doubt about the importance of this line */ 14 | %include "typemaps.i" 15 | 16 | %apply int *OUTPUT { int *state }; 17 | %apply double *OUTPUT { double *timestamp }; 18 | 19 | %define SWIG_PREPROCESSOR_SHOULD_SKIP_THIS %enddef 20 | 21 | %{ 22 | /* Includes the header in the wrapper code */ 23 | #include "ICartesianSolver.h" 24 | #include "ICartesianControl.h" 25 | %} 26 | 27 | /* Parse the header file to generate wrappers */ 28 | %include "ICartesianSolver.h" 29 | %include "ICartesianControl.h" 30 | 31 | %{ 32 | #include 33 | roboticslab::ICartesianSolver *viewICartesianSolver(yarp::dev::PolyDriver& d) 34 | { 35 | roboticslab::ICartesianSolver *result; 36 | d.view(result); 37 | return result; 38 | } 39 | %} 40 | extern roboticslab::ICartesianSolver *viewICartesianSolver(yarp::dev::PolyDriver& d); 41 | 42 | %{ 43 | #include 44 | roboticslab::ICartesianControl *viewICartesianControl(yarp::dev::PolyDriver& d) 45 | { 46 | roboticslab::ICartesianControl *result; 47 | d.view(result); 48 | return result; 49 | } 50 | %} 51 | extern roboticslab::ICartesianControl *viewICartesianControl(yarp::dev::PolyDriver& d); 52 | 53 | -------------------------------------------------------------------------------- /cmake/find-modules/FindGTestSources.cmake: -------------------------------------------------------------------------------- 1 | # Find the GTest headers and sources. 2 | # 3 | # Sets the following variables: 4 | # 5 | # * GTestSources_FOUND - system has GTest sources 6 | # * GTestSources_SOURCE_DIR - GTest source dir (with CMakeLists.txt) 7 | # * GTestSources_INCLUDE_DIR - GTest include directory (public headers) 8 | # * GTestSources_VERSION - GTest version (if supported) 9 | # 10 | # You can set the GTEST_ROOT environment variable to be used as a 11 | # hint by FindGTestSources to locate googletest source directory. 12 | # 13 | # Tested with the Ubuntu package `googletest` and the googletest 14 | # repository hosted on GitHub and cloned to the local machine. 15 | # 16 | # Supported versions: 1.8+. 17 | # 18 | # Also, this module adds the following macros: 19 | # 20 | # * gtest_add_tests (as in FindGTest.cmake) 21 | # * gtest_discover_tests (as in FindGTest.cmake) 22 | 23 | find_package(GTest QUIET) 24 | 25 | find_path(GTestSources_SOURCE_DIR googletest/src/gtest.cc 26 | HINTS $ENV{GTEST_ROOT} 27 | ${GTEST_ROOT} 28 | PATHS /usr/src/googletest) 29 | 30 | find_path(GTestSources_INCLUDE_DIR gtest/gtest.h 31 | HINTS ${GTestSources_SOURCE_DIR} 32 | $ENV{GTEST_ROOT} 33 | ${GTEST_ROOT} 34 | PATH_SUFFIXES googletest/include) 35 | 36 | set(GTestSources_VERSION GTestSources_VERSION-NOT_FOUND) 37 | 38 | if(GTestSources_SOURCE_DIR AND GTestSources_INCLUDE_DIR AND EXISTS ${GTestSources_SOURCE_DIR}/CMakeLists.txt) 39 | file(READ "${GTestSources_SOURCE_DIR}/CMakeLists.txt" _cmakelists) 40 | string(REGEX MATCH "GOOGLETEST_VERSION ([0-9]+\\.[0-9]+(\\.[0-9]+)?)" _match "${_cmakelists}") 41 | 42 | if(NOT "${CMAKE_MATCH_1}" STREQUAL "") 43 | set(GTestSources_VERSION "${CMAKE_MATCH_1}") 44 | elseif(EXISTS ${GTestSources_SOURCE_DIR}/googlemock/CMakeLists.txt) 45 | set(GTestSources_VERSION 1.8.0) 46 | endif() 47 | endif() 48 | 49 | include(FindPackageHandleStandardArgs) 50 | find_package_handle_standard_args(GTestSources FOUND_VAR GTestSources_FOUND 51 | REQUIRED_VARS GTestSources_SOURCE_DIR 52 | GTestSources_INCLUDE_DIR 53 | VERSION_VAR GTestSources_VERSION) 54 | 55 | if(GTestSources_FOUND) 56 | # should be non-cache variables, but CMP0077 strikes in 57 | set(BUILD_GMOCK OFF CACHE BOOL "") 58 | set(BUILD_GTEST ON CACHE BOOL "") 59 | set(INSTALL_GTEST OFF CACHE BOOL "") 60 | mark_as_advanced(BUILD_GMOCK BUILD_GTEST INSTALL_GTEST) 61 | endif() 62 | 63 | mark_as_advanced(GTestSources_SOURCE_DIR GTestSources_INCLUDE_DIR) 64 | -------------------------------------------------------------------------------- /doc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # https://stackoverflow.com/a/25217937 2 | get_directory_property(_has_parent PARENT_DIRECTORY) 3 | 4 | if(NOT _has_parent) 5 | cmake_minimum_required(VERSION 3.9) 6 | project(doxygen NONE) 7 | find_package(Doxygen REQUIRED) 8 | elseif(NOT DOXYGEN_FOUND) 9 | return() 10 | endif() 11 | 12 | set(DOXYGEN_PROJECT_NAME "kinematics-dynamics") 13 | set(DOXYGEN_REPEAT_BRIEF NO) 14 | set(DOXYGEN_EXTRACT_PRIVATE YES) 15 | set(DOXYGEN_EXTRACT_STATIC YES) 16 | set(DOXYGEN_EXAMPLE_PATH ./examples) 17 | set(DOXYGEN_EXAMPLE_RECURSIVE YES) 18 | set(DOXYGEN_IMAGE_PATH ./doc/fig) 19 | set(DOXYGEN_HTML_TIMESTAMP YES) 20 | set(DOXYGEN_USE_MATHJAX YES) 21 | set(DOXYGEN_USE_MDFILE_AS_MAINPAGE ./README.md) 22 | set(DOXYGEN_CITE_BIB_FILES ./doc/kinematics-dynamics.bib) 23 | 24 | set(_doxygen_input README.md 25 | doc 26 | examples 27 | libraries 28 | programs 29 | share 30 | tests) 31 | 32 | doxygen_add_docs(dox ${_doxygen_input} 33 | WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/..) 34 | -------------------------------------------------------------------------------- /doc/fig/kinematics-dynamics.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/roboticslab-uc3m/kinematics-dynamics/80a0269198a54269b61c5cc3de13abb90a90b8ec/doc/fig/kinematics-dynamics.png -------------------------------------------------------------------------------- /doc/groups.dox: -------------------------------------------------------------------------------- 1 | 2 | /** 3 | @brief The main, catch-all namespace for Robotics Lab UC3M. 4 | */ 5 | namespace roboticslab 6 | { 7 | /** 8 | * @brief Contains classes related to unit testing. 9 | */ 10 | namespace test {} 11 | } 12 | 13 | /** 14 | * \defgroup kinematics-dynamics-applications kinematics-dynamics Applications (Collections of Programs) 15 | * 16 | * @brief kinematics-dynamics applications (collections of programs). 17 | */ 18 | 19 | /** 20 | * \defgroup kinematics-dynamics-libraries kinematics-dynamics Libraries 21 | * 22 | * @brief kinematics-dynamics libraries. 23 | */ 24 | 25 | /** 26 | * @ingroup kinematics-dynamics-libraries 27 | * \defgroup YarpPlugins 28 | * 29 | * @brief Contains kinematics-dynamics libraries that implement YARP device interfaces 30 | * and therefore can be invoked as YARP plugins. 31 | */ 32 | 33 | /** 34 | * \defgroup kinematics-dynamics-programs kinematics-dynamics Programs 35 | * @brief kinematics-dynamics programs. 36 | */ 37 | 38 | /** 39 | * \defgroup kinematics-dynamics-examples kinematics-dynamics Examples 40 | * @brief kinematics-dynamics examples. 41 | */ 42 | 43 | /** 44 | * \defgroup kinematics-dynamics-tests kinematics-dynamics Tests 45 | * @brief kinematics-dynamics tests. 46 | */ 47 | -------------------------------------------------------------------------------- /doc/kinematics-dynamics-install.md: -------------------------------------------------------------------------------- 1 | # kinematics-dynamics: Installation from Source Code 2 | 3 | First install the dependencies: 4 | 5 | - [Install CMake 3.19+](https://github.com/roboticslab-uc3m/installation-guides/blob/master/install-cmake.md/) 6 | - [Install YCM 0.11+](https://github.com/roboticslab-uc3m/installation-guides/blob/master/install-ycm.md/) 7 | - [Install YARP 3.10+](https://github.com/roboticslab-uc3m/installation-guides/blob/master/install-yarp.md/) 8 | - [Install KDL 1.4+](https://github.com/roboticslab-uc3m/installation-guides/blob/master/install-kdl.md/) 9 | 10 | For unit testing, you'll need the googletest source package. Refer to [Install googletest](https://github.com/roboticslab-uc3m/installation-guides/blob/master/docs/install-googletest.md/). 11 | 12 | ## Install kinematics-dynamics on Ubuntu (working on all tested versions) 13 | 14 | Our software integrates the previous dependencies. Note that you will be prompted for your password upon using `sudo` a couple of times: 15 | 16 | ```bash 17 | cd # go home 18 | mkdir -p repos; cd repos # create $HOME/repos if it does not exist; then, enter it 19 | git clone https://github.com/roboticslab-uc3m/kinematics-dynamics.git # download kinematics-dynamics sources from GitHub 20 | cd kinematics-dynamics; mkdir build; cd build; cmake .. # configure the project 21 | make -j # compile 22 | sudo make install; sudo ldconfig # install 23 | ``` 24 | 25 | Use `ccmake` instead of `cmake` for additional options. 26 | 27 | ## Install Bindings 28 | 29 | Swig is needed in order to build all language bindings. Refer to [Install SWIG](https://github.com/roboticslab-uc3m/installation-guides/blob/master/docs/install-swig.md/). 30 | 31 | ### Install Python Bindings 32 | 33 | First, install Python development packages. 34 | 35 | ```bash 36 | sudo apt update 37 | sudo apt install libpython3-dev # not installed by default on clean distros 38 | ``` 39 | 40 | You can follow these steps after installing kinematics-dynamics, or just activate the correct CMake options during the initial build. 41 | 42 | ```bash 43 | cd # go home 44 | cd repos/kinematics-dynamics/build # this should already exist, see previous section 45 | cmake .. -DCREATE_PYTHON=ON -DCREATE_BINDINGS_PYTHON=ON # enable Python bindings 46 | make -j # compile 47 | sudo make install; sudo ldconfig; cd # install and go home 48 | ``` 49 | 50 | Note: You'll probably want [YARP Python bindings](https://github.com/roboticslab-uc3m/installation-guides/blob/master/docs/install-yarp.md/#install-python-bindings) ([perma](https://github.com/roboticslab-uc3m/installation-guides/blob/33c93b68ab34a63157b1dc940dfb154a8504fff8/install-yarp.md#install-python-bindings)), too. 51 | 52 | #### Install Python bindings (checking) 53 | 54 | Check your installation via (should output nothing; if bad, you will see a `ModuleNotFoundError`): 55 | 56 | ```bash 57 | python3 -c "import kinematics_dynamics" 58 | ``` 59 | 60 | #### Install Python bindings (troubleshooting) 61 | 62 | CMake may not detect the correct Python3 installation directory. Toggle `t` in `ccmake` to see additional configuration. The `CMAKE_INSTALL_PYTHONDIR` variable may point to a wrong path such as `lib/python3/dist-packages` (relative to `CMAKE_INSTALL_PREFIX`, which usually resolves to `/usr/local`). You must pick the python3.x directory instead (check via `python3 -V`); on Ubuntu 20.04 and Python 3.8, this configuration variable should be changed to `lib/python3.8/dist-packages`. 63 | 64 | ## Even more! 65 | 66 | Done! You are now probably interested in one of the following links: 67 | - [Simulation and Basic Control: Now what can I do?]( teo-post-install.md ) 68 | -------------------------------------------------------------------------------- /doc/kinematics-dynamics.bib: -------------------------------------------------------------------------------- 1 | @book{pardosgotor2018str, 2 | author = {José M. Pardos-Gotor}, 3 | title = {Screw Theory for Robotics: A practical approach for Modern Robot KINEMATICS}, 4 | publisher = {Amazon Fulfillment}, 5 | year = 2018, 6 | month = 9, 7 | note = {ISBN 978-1-7179-3181-8} 8 | } 9 | 10 | @book{pardosgotor2022str, 11 | author = {José M. Pardos-Gotor}, 12 | title = {Screw Theory for Robotics: An Illustrated and Practicable Introduction to Modern Mechanics}, 13 | publisher = {CRC Press}, 14 | year = 2022, 15 | note = {ISBN 978-1-032-10736-3} 16 | } 17 | 18 | @inproceedings{eona2020icarsc, 19 | author = {{O\~na}, Edwin Daniel and {\L}ukawski, Bartek and Jardón, Alberto and Balaguer, Carlos}, 20 | title = {A modular framework to facilitate the control of an assistive robotic arm using visual servoing and proximity sensing}, 21 | booktitle = {IEEE Int. Conf. on Autonomous Robot Systems and Competitions (ICARSC)}, 22 | year = {2020}, 23 | pages = {28--33}, 24 | doi = {10.1109/ICARSC49921.2020.9096146}, 25 | } 26 | 27 | @inproceedings{lukawski2022jjaa, 28 | author = {{\L}ukawski, Bartek and Montesino Valle, Ignacio and Victores, Juan G. and Jardón, Alberto and Balaguer, Carlos}, 29 | title = {An inverse kinematics problem solver based on screw theory for manipulator arms}, 30 | booktitle = {XLIII Jornadas de Automática}, 31 | year = {2022}, 32 | pages = {864--869}, 33 | publisher = {Universidade da Coruña}, 34 | doi = {10.17979/spudc.9788497498418.0864}, 35 | } 36 | 37 | @inproceedings{lukawski2023jjaa, 38 | author = {{\L}ukawski, Bartek and Victores, Juan G. and Balaguer, Carlos}, 39 | title = {A generic controller for teleoperation on robotic manipulators using low-cost devices}, 40 | booktitle = {XLIV Jornadas de Automática}, 41 | year = {2023}, 42 | pages = {785--788}, 43 | publisher = {Universidade da Coruña}, 44 | doi = {10.17979/spudc.9788497498609.785}, 45 | } 46 | -------------------------------------------------------------------------------- /examples/cpp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | option(ENABLE_examples "Enable/disable C++ examples" OFF) 2 | 3 | if(ENABLE_examples) 4 | if(TARGET ROBOTICSLAB::KinematicsDynamicsInterfaces) 5 | add_subdirectory(exampleCartesianControlClient) 6 | endif() 7 | 8 | if(TARGET ROBOTICSLAB::ScrewTheoryLib) 9 | add_subdirectory(exampleScrewTheoryTrajectory) 10 | endif() 11 | 12 | if(TARGET ROBOTICSLAB::YarpTinyMathLib) 13 | add_subdirectory(exampleYarpTinyMath) 14 | endif() 15 | endif() 16 | -------------------------------------------------------------------------------- /examples/cpp/exampleCartesianControlClient/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.19) 2 | 3 | project(exampleCartesianControlClient LANGUAGES CXX) 4 | 5 | if(NOT YARP_FOUND) 6 | find_package(YARP 3.10 REQUIRED COMPONENTS os dev) 7 | endif() 8 | 9 | if(NOT TARGET ROBOTICSLAB::KinematicsDynamicsInterfaces) 10 | find_package(ROBOTICSLAB_KINEMATICS_DYNAMICS REQUIRED) 11 | endif() 12 | 13 | add_executable(exampleCartesianControlClient exampleCartesianControlClient.cpp) 14 | 15 | target_link_libraries(exampleCartesianControlClient YARP::YARP_os 16 | YARP::YARP_init 17 | YARP::YARP_dev 18 | ROBOTICSLAB::KinematicsDynamicsInterfaces) 19 | -------------------------------------------------------------------------------- /examples/cpp/exampleScrewTheoryTrajectory/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.19) 2 | 3 | project(exampleScrewTheoryTrajectory LANGUAGES CXX) 4 | 5 | if(NOT YARP_FOUND) 6 | find_package(YARP 3.10 REQUIRED COMPONENTS os dev) 7 | endif() 8 | 9 | if(NOT TARGET ROBOTICSLAB::ScrewTheoryLib) 10 | find_package(ROBOTICSLAB_KINEMATICS_DYNAMICS REQUIRED) 11 | endif() 12 | 13 | find_package(orocos_kdl 1.4 QUIET) 14 | 15 | add_executable(exampleScrewTheoryTrajectory exampleScrewTheoryTrajectory.cpp 16 | TrajectoryThread.hpp 17 | TrajectoryThread.cpp) 18 | 19 | target_link_libraries(exampleScrewTheoryTrajectory YARP::YARP_os 20 | YARP::YARP_init 21 | YARP::YARP_dev 22 | ${orocos_kdl_LIBRARIES} 23 | ROBOTICSLAB::ScrewTheoryLib) 24 | 25 | target_include_directories(exampleScrewTheoryTrajectory PRIVATE ${orocos_kdl_INCLUDE_DIRS}) 26 | -------------------------------------------------------------------------------- /examples/cpp/exampleScrewTheoryTrajectory/TrajectoryThread.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include "TrajectoryThread.hpp" 4 | 5 | #include // std::none_of, std::transform 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | namespace 16 | { 17 | void printCartesianCoordinates(const KDL::Frame & H, double movementTime) 18 | { 19 | KDL::Vector p = H.p; 20 | double roll, pitch, yaw; 21 | H.M.GetRPY(roll, pitch, yaw); 22 | 23 | yInfo("GOAL -> p: %f %f %f, r: %f %f %f [%f]", p.x(), p.y(), p.z(), roll, pitch, yaw, movementTime); 24 | } 25 | } 26 | 27 | bool TrajectoryThread::threadInit() 28 | { 29 | startTime = yarp::os::Time::now(); 30 | return iEncoders->getAxes(&axes); 31 | } 32 | 33 | void TrajectoryThread::run() 34 | { 35 | double movementTime = yarp::os::Time::now() - startTime; 36 | KDL::Frame H_S_T = trajectory->Pos(movementTime); 37 | 38 | printCartesianCoordinates(H_S_T, movementTime); 39 | 40 | std::vector solutions; 41 | auto reachability = ikProblem->solve(H_S_T, solutions); 42 | 43 | if (std::none_of(reachability.begin(), reachability.end(), [](bool r) { return r; })) 44 | { 45 | yWarning() << "IK exact solution not found"; 46 | } 47 | 48 | if (!ikConfig->configure(solutions, reachability)) 49 | { 50 | yError() << "IK solutions out of joint limits"; 51 | return; 52 | } 53 | 54 | KDL::JntArray q(axes); 55 | 56 | if (!iEncoders->getEncoders(q.data.data())) 57 | { 58 | yError() << "getEncoders() failed"; 59 | return; 60 | } 61 | 62 | for (int i = 0; i < q.rows(); i++) 63 | { 64 | q(i) = KDL::deg2rad * q(i); 65 | } 66 | 67 | if (!ikConfig->findOptimalConfiguration(q)) 68 | { 69 | yError() << "Optimal configuration not found"; 70 | return; 71 | } 72 | 73 | KDL::JntArray solution; 74 | 75 | ikConfig->retrievePose(solution); 76 | 77 | std::vector refs(solution.data.data(), solution.data.data() + solution.data.size()); 78 | std::transform(refs.begin(), refs.end(), refs.begin(), [](const auto & v) { return v * KDL::rad2deg; }); 79 | yInfo() << "IK ->" << refs; 80 | 81 | iPosDirect->setPositions(refs.data()); 82 | } 83 | -------------------------------------------------------------------------------- /examples/cpp/exampleScrewTheoryTrajectory/TrajectoryThread.hpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #ifndef __TRAJECTORY_THREAD_HPP__ 4 | #define __TRAJECTORY_THREAD_HPP__ 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | class TrajectoryThread : public yarp::os::PeriodicThread 17 | { 18 | public: 19 | TrajectoryThread(yarp::dev::IEncoders * iEncoders, 20 | yarp::dev::IPositionDirect * iPosDirect, 21 | roboticslab::ScrewTheoryIkProblem * ikProblem, 22 | roboticslab::ConfigurationSelector * ikConfig, 23 | KDL::Trajectory * trajectory, 24 | int period) 25 | : yarp::os::PeriodicThread(period * 0.001, yarp::os::PeriodicThreadClock::Absolute), 26 | iEncoders(iEncoders), 27 | iPosDirect(iPosDirect), 28 | ikProblem(ikProblem), 29 | ikConfig(ikConfig), 30 | trajectory(trajectory), 31 | axes(0), 32 | startTime(0) 33 | {} 34 | 35 | protected: 36 | bool threadInit() override; 37 | void run() override; 38 | 39 | private: 40 | yarp::dev::IEncoders * iEncoders; 41 | yarp::dev::IPositionDirect * iPosDirect; 42 | roboticslab::ScrewTheoryIkProblem * ikProblem; 43 | roboticslab::ConfigurationSelector * ikConfig; 44 | KDL::Trajectory * trajectory; 45 | int axes; 46 | double startTime; 47 | }; 48 | 49 | #endif // __TRAJECTORY_THREAD_HPP__ 50 | -------------------------------------------------------------------------------- /examples/cpp/exampleYarpTinyMath/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.19) 2 | 3 | project(exampleYarpTinyMath LANGUAGES CXX) 4 | 5 | if(NOT YARP_math_FOUND) 6 | find_package(YARP 3.10 REQUIRED COMPONENTS sig math) 7 | endif() 8 | 9 | if(NOT TARGET ROBOTICSLAB::YarpTinyMathLib) 10 | find_package(ROBOTICSLAB_KINEMATICS_DYNAMICS REQUIRED) 11 | endif() 12 | 13 | add_executable(exampleYarpTinyMath exampleYarpTinyMath.cpp) 14 | 15 | target_link_libraries(exampleYarpTinyMath YARP::YARP_sig 16 | YARP::YARP_math 17 | ROBOTICSLAB::YarpTinyMathLib) 18 | -------------------------------------------------------------------------------- /examples/cpp/exampleYarpTinyMath/exampleYarpTinyMath.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | /** 4 | * @ingroup examples_cpp 5 | * 6 | * \defgroup exampleYarpTinyMath exampleYarpTinyMath 7 | * 8 | * @brief This is an example of the use of YarpTinyMath. 9 | * 10 | * @section exampleYarpTinyMath_legal Legal 11 | * 12 | * Copyright: (C) 2013 Universidad Carlos III de Madrid 13 | * 14 | * Author: Juan G. Victores 15 | * 16 | * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see license/LGPL.TXT 17 | * 18 | * @section exampleYarpTinyMath_build Building 19 | \verbatim 20 | cd examples/cpp/exampleYarpTinyMath/ 21 | mkdir build; cd build; cmake .. 22 | make -j3 23 | \endverbatim 24 | * 25 | * @section exampleYarpTinyMath_run Running 26 | \verbatim 27 | ./exampleYarpTinyMath 28 | \endverbatim 29 | * 30 | */ 31 | 32 | #include 33 | 34 | #include 35 | #include // operators 36 | 37 | #include 38 | 39 | int main(int argc, char *argv[]) 40 | { 41 | double x_k = 0.0; 42 | double y_k = 0.0; 43 | double z_k = 1.0; 44 | 45 | double height = 0.1; 46 | double tilt = 90.0; // 90 deg from vertical = horizontal std kinect looking position. 47 | double pan = 10.0; // deg, from front 48 | 49 | std::printf("Lets say a 3D sensor grabbed a point at sensor coordinates %f, %f, %f.\n\n", x_k, y_k, z_k); 50 | 51 | yarp::sig::Matrix H_0_c = roboticslab::rotZ(pan); 52 | H_0_c.resize(4, 4); 53 | H_0_c(3, 3) = 1.0; 54 | std::printf("*** H_0_c *** \n(%s)\n\n", H_0_c.toString().c_str()); 55 | 56 | yarp::sig::Matrix H_c_k = roboticslab::rotY(tilt); 57 | H_c_k.resize(4, 4); 58 | H_c_k(2, 3) = height; 59 | H_c_k(3, 3) = 1.0; 60 | std::printf("*** H_c_k *** \n(%s)\n\n", H_c_k.toString().c_str()); 61 | 62 | yarp::sig::Matrix H_k_P = yarp::math::eye(4, 4); 63 | H_k_P(0, 3) = x_k; 64 | H_k_P(1, 3) = y_k; 65 | H_k_P(2, 3) = z_k; 66 | std::printf("*** H_k_P *** \n(%s)\n\n", H_k_P.toString().c_str()); 67 | 68 | yarp::sig::Matrix H_0_P = H_0_c * H_c_k * H_k_P; 69 | 70 | double x_0 = H_0_P(0, 3); 71 | double y_0 = H_0_P(1, 3); 72 | double z_0 = H_0_P(2, 3); 73 | 74 | std::printf("In origin coordinates this would be: %f, %f, %f.\n", x_0, y_0, z_0); 75 | 76 | std::printf("Bye!\n"); 77 | 78 | return 0; 79 | } 80 | -------------------------------------------------------------------------------- /examples/python/exampleCartesianControlClient.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import yarp 4 | import roboticslab_kinematics_dynamics as kd 5 | 6 | yarp.Network.init() 7 | 8 | if not yarp.Network.checkNetwork(): 9 | print('[error] Please try running yarp server') 10 | raise SystemExit 11 | 12 | options = yarp.Property() 13 | options.put('device', 'CartesianControlClient') 14 | options.put('cartesianRemote', '/teoSim/rightArm/CartesianControl') 15 | options.put('cartesianLocal', '/cartesianControlExample') 16 | 17 | dd = yarp.PolyDriver(options) 18 | 19 | if not dd.isValid(): 20 | print('Cannot open the device!') 21 | raise SystemExit 22 | 23 | cartesianControl = kd.viewICartesianControl(dd) 24 | 25 | print('> stat') 26 | x = yarp.DVector() 27 | ret, state, ts = cartesianControl.stat(x) 28 | print('<', yarp.decode(state), '[%s]' % ', '.join(map(str, x))) 29 | 30 | xd = [ 31 | [0.4025, -0.3469, 0.1692, 0.0, 1.5708, 0.0], 32 | [0.5, -0.3469, 0.1692, 0.0, 1.5708, 0.0], 33 | [0.5, -0.4, 0.1692, 0.0, 1.5708, 0.0], 34 | [0.5, -0.4, 0.1692, 0.0, 1.36, 0.0], 35 | [0.5, -0.4, 0.1692, 0.6139, 1.4822, 0.6139], 36 | [0.4025, -0.3469, 0.1692, 0.0, 1.5708, 0.0], 37 | [0.0, -0.3469, -0.2333, 0.0, 3.1416, 0.0] 38 | ] 39 | 40 | for i in range(len(xd)): 41 | print('-- movement ' + str(i + 1) + ':') 42 | print('> inv [%s]' % ', '.join(map(str, xd[i]))) 43 | xd_vector = yarp.DVector(xd[i]) 44 | qd_vector = yarp.DVector() 45 | 46 | if cartesianControl.inv(xd_vector, qd_vector): 47 | print('< [%s]' % ', '.join(map(str, qd_vector))) 48 | else: 49 | print('< [fail]') 50 | continue 51 | 52 | print('> movj [%s]' % ', '.join(map(str, xd[i]))) 53 | xd_vector = yarp.DVector(xd[i]) 54 | 55 | if cartesianControl.movj(xd_vector): 56 | print('< [ok]') 57 | print('< [wait...]') 58 | cartesianControl.wait() 59 | else: 60 | print('< [fail]') 61 | 62 | print('bye!') 63 | -------------------------------------------------------------------------------- /examples/python/exampleKdlSolverFromFile.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import yarp 4 | import roboticslab_kinematics_dynamics as kd 5 | 6 | #-- Locate the file with the kinematic chain DH parameters 7 | 8 | rf = yarp.ResourceFinder() 9 | rf.setVerbose(False) 10 | rf.setDefaultContext("testKdlSolverFromFile") 11 | rf.setDefaultConfigFile("testKdlSolverFromFile.ini") 12 | kinematicsFileFullPath = rf.findFileByName("testKdlSolverFromFile.ini") 13 | 14 | #-- Load the solver device with this configuration, view the interface 15 | 16 | solverOptions = yarp.Property() 17 | solverOptions.fromConfigFile(kinematicsFileFullPath) 18 | solverOptions.put("device","KdlSolver") 19 | solverOptions.fromString("(mins (-70 -15 -10 -100 -90 -100)) (maxs (45 70 75 10 90 10))", False) 20 | 21 | solverDevice = yarp.PolyDriver(solverOptions) 22 | 23 | if not solverDevice.isValid(): 24 | print("Cannot open the device!") 25 | raise SystemExit 26 | 27 | cartesianSolver = kd.viewICartesianSolver(solverDevice) # view the actual interface 28 | 29 | ##-- Illustration of forward kinematics, for different joint-space positions 30 | 31 | print('--- Joint space configuration 1: expect Cartesian space position 0, 0.34692, -0.221806') 32 | 33 | q = [0,0,0,0,0,0,0] 34 | q_vector = yarp.DVector(q) 35 | x_vector = yarp.DVector() 36 | 37 | cartesianSolver.fwdKin(q_vector,x_vector); 38 | 39 | print('> fwdKin [%s]' % ', '.join(map(str, q_vector))) 40 | print('< [%s]' % ', '.join(map(str, x_vector))) 41 | 42 | print('--- Joint space configuration 2: expect Cartesian space position 0.718506, 0.34692, 0.4967') 43 | 44 | q = [-90,0,0,0,0,0,0] 45 | q_vector = yarp.DVector(q) 46 | x_vector = yarp.DVector() 47 | 48 | cartesianSolver.fwdKin(q_vector,x_vector); 49 | 50 | print('> fwdKin [%s]' % ', '.join(map(str, q_vector))) 51 | print('< [%s]' % ', '.join(map(str, x_vector))) 52 | 53 | print('bye!') 54 | -------------------------------------------------------------------------------- /libraries/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(KdlVectorConverterLib) 2 | add_subdirectory(KinematicRepresentationLib) 3 | add_subdirectory(ScrewTheoryLib) 4 | add_subdirectory(YarpPlugins) 5 | add_subdirectory(YarpTinyMathLib) 6 | 7 | # depends on CMake options defined in YarpPlugins 8 | add_subdirectory(KdlSolverUtils) 9 | -------------------------------------------------------------------------------- /libraries/KdlSolverUtils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(ENABLE_KdlSolver OR ENABLE_KdlTreeSolver) 2 | add_library(KdlSolverUtils OBJECT KdlSolverUtils.hpp 3 | KdlSolverUtils.cpp) 4 | 5 | set_target_properties(KdlSolverUtils PROPERTIES POSITION_INDEPENDENT_CODE TRUE) 6 | 7 | target_link_libraries(KdlSolverUtils PUBLIC YARP::YARP_os 8 | ${orocos_kdl_LIBRARIES}) 9 | 10 | target_include_directories(KdlSolverUtils PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} 11 | ${orocos_kdl_INCLUDE_DIRS}) 12 | 13 | add_library(ROBOTICSLAB::KdlSolverUtils ALIAS KdlSolverUtils) 14 | endif() 15 | -------------------------------------------------------------------------------- /libraries/KdlSolverUtils/KdlSolverUtils.hpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #ifndef __KDL_SOLVER_UTILS_HPP__ 4 | #define __KDL_SOLVER_UTILS_HPP__ 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | namespace roboticslab::KdlSolverUtils 15 | { 16 | 17 | /** 18 | * @ingroup kinematics-dynamics-libraries 19 | * @defgroup KdlSolverUtils 20 | * 21 | * @brief Contains utilities related to @ref KdlSolver and @ref KdlTreeSolver. 22 | */ 23 | 24 | bool parseKinematicsConfig(const yarp::os::Searchable & config, const std::string & filename, yarp::os::Property & kinematicsConfig); 25 | 26 | Eigen::MatrixXd getMatrixFromVector(const std::vector & v); 27 | 28 | KDL::JntArray getJntArrayFromVector(const std::vector in); 29 | 30 | bool makeChain(const yarp::os::Searchable & kinConfig, KDL::Chain & chain); 31 | 32 | } // namespace roboticslab::KdlSolverUtils 33 | 34 | #endif // __KDL_SOLVER_UTILS_HPP__ 35 | -------------------------------------------------------------------------------- /libraries/KdlVectorConverterLib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(NOT orocos_kdl_FOUND AND (NOT DEFINED ENABLE_KdlVectorConverterLib OR ENABLE_KdlVectorConverterLib)) 2 | message(WARNING "orocos_kdl package not found, disabling KdlVectorConverterLib") 3 | endif() 4 | 5 | cmake_dependent_option(ENABLE_KdlVectorConverterLib "Enable/disable KdlVectorConverterLib library" ON 6 | orocos_kdl_FOUND OFF) 7 | 8 | if(ENABLE_KdlVectorConverterLib) 9 | 10 | # Set up our library. 11 | add_library(KdlVectorConverterLib SHARED KdlVectorConverter.cpp 12 | KdlVectorConverter.hpp) 13 | 14 | set_target_properties(KdlVectorConverterLib PROPERTIES PUBLIC_HEADER KdlVectorConverter.hpp) 15 | 16 | target_link_libraries(KdlVectorConverterLib PUBLIC ${orocos_kdl_LIBRARIES} 17 | PRIVATE YARP::YARP_os) 18 | 19 | target_include_directories(KdlVectorConverterLib PUBLIC ${orocos_kdl_INCLUDE_DIRS} 20 | $ 21 | $) 22 | 23 | target_compile_features(KdlVectorConverterLib PUBLIC cxx_std_17) # nested namespace definition 24 | 25 | install(TARGETS KdlVectorConverterLib 26 | EXPORT ROBOTICSLAB_KINEMATICS_DYNAMICS) 27 | 28 | set_property(GLOBAL APPEND PROPERTY _exported_dependencies orocos_kdl) 29 | 30 | add_library(ROBOTICSLAB::KdlVectorConverterLib ALIAS KdlVectorConverterLib) 31 | 32 | else() 33 | 34 | set(ENABLE_KdlVectorConverterLib OFF CACHE BOOL "Enable/disable KdlVectorConverterLib library" FORCE) 35 | 36 | endif() 37 | -------------------------------------------------------------------------------- /libraries/KdlVectorConverterLib/KdlVectorConverter.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include "KdlVectorConverter.hpp" 4 | 5 | #include 6 | #include 7 | 8 | namespace 9 | { 10 | YARP_LOG_COMPONENT(KDLVC, "rl.KdlVectorConverter") 11 | } 12 | 13 | namespace roboticslab::KdlVectorConverter 14 | { 15 | 16 | // ----------------------------------------------------------------------------- 17 | 18 | KDL::Frame vectorToFrame(const std::vector &x) 19 | { 20 | if (x.size() != 6) 21 | { 22 | yCWarning(KDLVC, "Size mismatch; expected: 6, was: %zu", x.size()); 23 | return KDL::Frame::Identity(); 24 | } 25 | 26 | KDL::Frame f; 27 | 28 | f.p.x(x[0]); 29 | f.p.y(x[1]); 30 | f.p.z(x[2]); 31 | 32 | KDL::Vector rotvec(x[3], x[4], x[5]); 33 | 34 | f.M = KDL::Rotation::Rot(rotvec, rotvec.Norm()); 35 | 36 | return f; 37 | } 38 | 39 | // ----------------------------------------------------------------------------- 40 | 41 | std::vector frameToVector(const KDL::Frame& f) 42 | { 43 | std::vector x(6); 44 | 45 | x[0] = f.p.x(); 46 | x[1] = f.p.y(); 47 | x[2] = f.p.z(); 48 | 49 | KDL::Vector rotVector = f.M.GetRot(); 50 | 51 | x[3] = rotVector.x(); 52 | x[4] = rotVector.y(); 53 | x[5] = rotVector.z(); 54 | 55 | return x; 56 | } 57 | 58 | // ----------------------------------------------------------------------------- 59 | 60 | KDL::Twist vectorToTwist(const std::vector &xdot) 61 | { 62 | if (xdot.size() != 6) 63 | { 64 | yCWarning(KDLVC, "Size mismatch; expected: 6, was: %zu", xdot.size()); 65 | return KDL::Twist::Zero(); 66 | } 67 | 68 | KDL::Twist t; 69 | 70 | t.vel.x(xdot[0]); 71 | t.vel.y(xdot[1]); 72 | t.vel.z(xdot[2]); 73 | 74 | t.rot.x(xdot[3]); 75 | t.rot.y(xdot[4]); 76 | t.rot.z(xdot[5]); 77 | 78 | return t; 79 | } 80 | 81 | // ----------------------------------------------------------------------------- 82 | 83 | std::vector twistToVector(const KDL::Twist& t) 84 | { 85 | std::vector xdot(6); 86 | 87 | xdot[0] = t.vel.x(); 88 | xdot[1] = t.vel.y(); 89 | xdot[2] = t.vel.z(); 90 | 91 | xdot[3] = t.rot.x(); 92 | xdot[4] = t.rot.y(); 93 | xdot[5] = t.rot.z(); 94 | 95 | return xdot; 96 | } 97 | 98 | // ----------------------------------------------------------------------------- 99 | 100 | KDL::Wrench vectorToWrench(const std::vector & f) 101 | { 102 | if (f.size() != 6) 103 | { 104 | yCWarning(KDLVC, "Size mismatch; expected: 6, was: %zu", f.size()); 105 | return KDL::Wrench::Zero(); 106 | } 107 | 108 | KDL::Wrench w; 109 | 110 | w.force.x(f[0]); 111 | w.force.y(f[1]); 112 | w.force.z(f[2]); 113 | 114 | w.torque.x(f[3]); 115 | w.torque.y(f[4]); 116 | w.torque.z(f[5]); 117 | 118 | return w; 119 | } 120 | 121 | // ----------------------------------------------------------------------------- 122 | 123 | std::vector wrenchToVector(const KDL::Wrench & w) 124 | { 125 | std::vector f(6); 126 | 127 | f[0] = w.force.x(); 128 | f[1] = w.force.y(); 129 | f[2] = w.force.z(); 130 | 131 | f[3] = w.torque.x(); 132 | f[4] = w.torque.y(); 133 | f[5] = w.torque.z(); 134 | 135 | return f; 136 | } 137 | 138 | // ----------------------------------------------------------------------------- 139 | 140 | } // namespace roboticslab::KdlVectorConverter 141 | -------------------------------------------------------------------------------- /libraries/KdlVectorConverterLib/KdlVectorConverter.hpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #ifndef __KDL_VECTOR_CONVERTER_HPP__ 4 | #define __KDL_VECTOR_CONVERTER_HPP__ 5 | 6 | #include 7 | 8 | #include 9 | 10 | /** 11 | * @ingroup kinematics-dynamics-libraries 12 | * \defgroup KdlVectorConverterLib 13 | * 14 | * @brief Contains classes related to KDL and std::vector classes. 15 | */ 16 | 17 | /** 18 | * @ingroup KdlVectorConverterLib 19 | * @brief Collection of utilities related to KDL and std::vector classes. 20 | */ 21 | namespace roboticslab::KdlVectorConverter 22 | { 23 | 24 | /** 25 | * @brief Convert from std::vector to KDL::Frame 26 | * 27 | * @param x 6-element vector describing a position in cartesian space; first 28 | * three elements denote translation (meters), last three denote rotation in 29 | * scaled axis-angle representation (radians). 30 | * 31 | * @return Resulting KDL::Frame object. 32 | */ 33 | KDL::Frame vectorToFrame(const std::vector & x); 34 | 35 | /** 36 | * @brief Convert from KDL::Frame to std::vector 37 | * 38 | * @param f Input KDL::Frame object. 39 | * 40 | * @return Resulting 6-element vector describing a position in cartesian space; first 41 | * three elements denote translation (meters), last three denote rotation in scaled 42 | * axis-angle representation (radians). 43 | */ 44 | std::vector frameToVector(const KDL::Frame & f); 45 | 46 | /** 47 | * @brief Convert from std::vector to KDL::Twist 48 | * 49 | * @param xdot 6-element vector describing a velocity in cartesian space; first 50 | * three elements denote translational velocity (meters/second), last three denote 51 | * angular velocity (radians/second). 52 | * 53 | * @return Resulting KDL::Twist object. 54 | */ 55 | KDL::Twist vectorToTwist(const std::vector & xdot); 56 | 57 | /** 58 | * @brief Convert from KDL::Twist to std::vector 59 | * 60 | * @param t Input KDL::Twist object 61 | * 62 | * @return Resulting 6-element vector describing a velocity in cartesian space; first 63 | * three elements denote translational velocity (meters/second), last three denote 64 | * angular velocity (radians/second). 65 | */ 66 | std::vector twistToVector(const KDL::Twist & t); 67 | 68 | /** 69 | * @brief Convert from std::vector to KDL::Wrench 70 | * 71 | * @param xdot 6-element vector describing a wrench in cartesian space; first 72 | * three elements denote force (N), last three denote torque (Nm). 73 | * 74 | * @return Resulting KDL::Wrench object. 75 | */ 76 | KDL::Wrench vectorToWrench(const std::vector & f); 77 | 78 | /** 79 | * @brief Convert from KDL::Wrench to std::vector 80 | * 81 | * @param t Input KDL::Wrench object 82 | * 83 | * @return Resulting 6-element vector describing a wrench in cartesian space; first 84 | * three elements denote force (N), last three denote torque (Nm). 85 | */ 86 | std::vector wrenchToVector(const KDL::Wrench & w); 87 | 88 | } // namespace roboticslab::KdlVectorConverter 89 | 90 | #endif // __KDL_VECTOR_CONVERTER_HPP__ 91 | -------------------------------------------------------------------------------- /libraries/KinematicRepresentationLib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(NOT orocos_kdl_FOUND AND (NOT DEFINED ENABLE_KinematicRepresentationLib OR ENABLE_KinematicRepresentationLib)) 2 | message(WARNING "orocos_kdl package not found, disabling KinematicRepresentationLib") 3 | endif() 4 | 5 | cmake_dependent_option(ENABLE_KinematicRepresentationLib "Enable/disable KinematicRepresentationLib library" ON 6 | orocos_kdl_FOUND OFF) 7 | 8 | if(ENABLE_KinematicRepresentationLib) 9 | 10 | add_library(KinematicRepresentationLib SHARED KinematicRepresentation.hpp 11 | KinematicRepresentation.cpp) 12 | 13 | set_target_properties(KinematicRepresentationLib PROPERTIES PUBLIC_HEADER KinematicRepresentation.hpp) 14 | 15 | target_link_libraries(KinematicRepresentationLib PRIVATE ${orocos_kdl_LIBRARIES} 16 | YARP::YARP_os) 17 | 18 | target_include_directories(KinematicRepresentationLib PUBLIC $ 19 | $ 20 | PRIVATE ${orocos_kdl_INCLUDE_DIRS}) 21 | 22 | target_compile_features(KinematicRepresentationLib PUBLIC cxx_std_17) # nested namespace definition 23 | 24 | install(TARGETS KinematicRepresentationLib 25 | EXPORT ROBOTICSLAB_KINEMATICS_DYNAMICS) 26 | 27 | add_library(ROBOTICSLAB::KinematicRepresentationLib ALIAS KinematicRepresentationLib) 28 | 29 | else() 30 | 31 | set(ENABLE_KinematicRepresentationLib OFF CACHE BOOL "Enable/disable KinematicRepresentationLib library" FORCE) 32 | 33 | endif() 34 | -------------------------------------------------------------------------------- /libraries/ScrewTheoryLib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(NOT orocos_kdl_FOUND AND (NOT DEFINED ENABLE_ScrewTheoryLib OR ENABLE_ScrewTheoryLib)) 2 | message(WARNING "orocos_kdl package not found, disabling ScrewTheoryLib") 3 | endif() 4 | 5 | cmake_dependent_option(ENABLE_ScrewTheoryLib "Enable/disable ScrewTheoryLib library" ON 6 | orocos_kdl_FOUND OFF) 7 | 8 | if(ENABLE_ScrewTheoryLib) 9 | 10 | add_library(ScrewTheoryLib SHARED ScrewTheoryTools.hpp 11 | ScrewTheoryTools.cpp 12 | MatrixExponential.hpp 13 | MatrixExponential.cpp 14 | ProductOfExponentials.hpp 15 | ProductOfExponentials.cpp 16 | ScrewTheoryIkProblem.hpp 17 | ScrewTheoryIkProblem.cpp 18 | ScrewTheoryIkProblemBuilder.cpp 19 | ScrewTheoryIkSubproblems.hpp 20 | PadenKahanSubproblems.cpp 21 | PardosGotorSubproblems.cpp 22 | ConfigurationSelector.hpp 23 | ConfigurationSelector.cpp 24 | ConfigurationSelectorLeastOverallAngularDisplacement.cpp 25 | ConfigurationSelectorHumanoidGait.cpp 26 | LogComponent.hpp 27 | LogComponent.cpp) 28 | 29 | set_property(TARGET ScrewTheoryLib PROPERTY PUBLIC_HEADER MatrixExponential.hpp 30 | ProductOfExponentials.hpp 31 | ScrewTheoryIkProblem.hpp 32 | ConfigurationSelector.hpp) 33 | 34 | target_link_libraries(ScrewTheoryLib PUBLIC ${orocos_kdl_LIBRARIES} 35 | PRIVATE YARP::YARP_os) 36 | 37 | target_include_directories(ScrewTheoryLib PUBLIC ${orocos_kdl_INCLUDE_DIRS} 38 | $ 39 | $) 40 | 41 | target_compile_features(ScrewTheoryLib PUBLIC cxx_std_17) # nested namespace definition 42 | 43 | install(TARGETS ScrewTheoryLib 44 | EXPORT ROBOTICSLAB_KINEMATICS_DYNAMICS) 45 | 46 | set_property(GLOBAL APPEND PROPERTY _exported_dependencies orocos_kdl) 47 | 48 | add_library(ROBOTICSLAB::ScrewTheoryLib ALIAS ScrewTheoryLib) 49 | 50 | else() 51 | 52 | set(ENABLE_ScrewTheoryLib OFF CACHE BOOL "Enable/disable ScrewTheoryLib library" FORCE) 53 | 54 | endif() 55 | -------------------------------------------------------------------------------- /libraries/ScrewTheoryLib/ConfigurationSelector.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include "ConfigurationSelector.hpp" 4 | 5 | using namespace roboticslab; 6 | 7 | // ----------------------------------------------------------------------------- 8 | 9 | bool ConfigurationSelector::configure(const std::vector & solutions, const std::vector & reachability) 10 | { 11 | configs.resize(solutions.size()); 12 | 13 | bool anyValid = false; 14 | 15 | for (int i = 0; i < solutions.size(); i++) 16 | { 17 | configs[i].store(&solutions[i]); 18 | anyValid |= reachability[i] && validate(configs[i]); 19 | } 20 | 21 | return anyValid; 22 | } 23 | 24 | // ----------------------------------------------------------------------------- 25 | 26 | bool ConfigurationSelector::validate(Configuration & config) 27 | { 28 | const auto * q = config.retrievePose(); 29 | 30 | for (int i = 0; i < q->rows(); i++) 31 | { 32 | const auto & _q = *q; // avoid calling q->operator()(i) 33 | 34 | if (!checkJointInLimits(_q(i), _qMin(i), _qMax(i))) 35 | { 36 | config.invalidate(); 37 | return false; 38 | } 39 | } 40 | 41 | return true; 42 | } 43 | 44 | // ----------------------------------------------------------------------------- 45 | -------------------------------------------------------------------------------- /libraries/ScrewTheoryLib/ConfigurationSelectorHumanoidGait.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include "ConfigurationSelector.hpp" 4 | 5 | #include // std::abs 6 | 7 | #include 8 | 9 | using namespace roboticslab; 10 | 11 | bool ConfigurationSelectorHumanoidGait::findOptimalConfiguration(const KDL::JntArray & qGuess) 12 | { 13 | if (qGuess.rows() != 6) 14 | { 15 | return false; 16 | } 17 | 18 | for (int i = 0; i < configs.size(); i++) 19 | { 20 | if (configs[i].isValid() && !applyConstraints(configs[i])) 21 | { 22 | configs[i].invalidate(); 23 | } 24 | } 25 | 26 | return ConfigurationSelectorLeastOverallAngularDisplacement::findOptimalConfiguration(qGuess); 27 | } 28 | 29 | bool ConfigurationSelectorHumanoidGait::applyConstraints(const Configuration & config) 30 | { 31 | const auto & q = *config.retrievePose(); 32 | 33 | if (std::abs(q(0)) > KDL::PI_2) 34 | { 35 | return false; 36 | } 37 | 38 | if (std::abs(q(1)) > KDL::PI_2) 39 | { 40 | return false; 41 | } 42 | 43 | if (std::abs(q(2)) > KDL::PI_2) 44 | { 45 | return false; 46 | } 47 | 48 | if (q(3) < 0.0) 49 | { 50 | return false; 51 | } 52 | 53 | if (std::abs(q(4)) > KDL::PI_2) 54 | { 55 | return false; 56 | } 57 | 58 | if (std::abs(q(5)) > KDL::PI_2) 59 | { 60 | return false; 61 | } 62 | 63 | return true; 64 | } 65 | -------------------------------------------------------------------------------- /libraries/ScrewTheoryLib/ConfigurationSelectorLeastOverallAngularDisplacement.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include "ConfigurationSelector.hpp" 4 | 5 | #include // std::abs 6 | #include // std::accumulate 7 | #include 8 | #include // std::pair 9 | 10 | using namespace roboticslab; 11 | 12 | bool ConfigurationSelectorLeastOverallAngularDisplacement::findOptimalConfiguration(const KDL::JntArray & qGuess) 13 | { 14 | if (lastValid != INVALID_CONFIG) 15 | { 16 | // either keep last valid configuration or skip looking (out of reach) 17 | return configs[lastValid].isValid(); 18 | } 19 | 20 | // best for all revolute/prismatic joints 21 | std::set> displacementPerConfiguration; 22 | 23 | for (int i = 0; i < configs.size(); i++) 24 | { 25 | if (configs[i].isValid()) 26 | { 27 | const auto diffs = getDiffs(qGuess, configs[i]); 28 | double sum = std::accumulate(diffs.begin(), diffs.end(), 0.0); 29 | displacementPerConfiguration.emplace(sum, i); 30 | } 31 | } 32 | 33 | if (displacementPerConfiguration.empty()) 34 | { 35 | // no valid configuration found 36 | return false; 37 | } 38 | 39 | // std::map keys are sorted, pick std::pair with lowest key (angle sum) 40 | auto it = displacementPerConfiguration.begin(); 41 | lastValid = it->second; 42 | 43 | return true; 44 | } 45 | 46 | std::vector ConfigurationSelectorLeastOverallAngularDisplacement::getDiffs(const KDL::JntArray & qGuess, const Configuration & config) 47 | { 48 | std::vector diffs; 49 | diffs.reserve(qGuess.rows()); 50 | 51 | for (int i = 0; i < qGuess.rows(); i++) 52 | { 53 | const auto & q = *config.retrievePose(); 54 | diffs.push_back(std::abs(qGuess(i) - q(i))); 55 | } 56 | 57 | return diffs; 58 | } 59 | -------------------------------------------------------------------------------- /libraries/ScrewTheoryLib/LogComponent.cpp: -------------------------------------------------------------------------------- 1 | #include "LogComponent.hpp" 2 | 3 | YARP_LOG_COMPONENT(ST, "rl.ScrewTheory") 4 | -------------------------------------------------------------------------------- /libraries/ScrewTheoryLib/LogComponent.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __SCREW_THEORY_LOG_COMPONENT_HPP__ 2 | #define __SCREW_THEORY_LOG_COMPONENT_HPP__ 3 | 4 | #include 5 | 6 | YARP_DECLARE_LOG_COMPONENT(ST) 7 | 8 | #endif // __SCREW_THEORY_LOG_COMPONENT_HPP__ 9 | -------------------------------------------------------------------------------- /libraries/ScrewTheoryLib/MatrixExponential.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include "MatrixExponential.hpp" 4 | 5 | #include 6 | 7 | #include "LogComponent.hpp" 8 | #include "ScrewTheoryTools.hpp" 9 | 10 | using namespace roboticslab; 11 | 12 | // ----------------------------------------------------------------------------- 13 | 14 | namespace 15 | { 16 | inline KDL::Rotation operator-(const KDL::Rotation & lhs, const KDL::Rotation & rhs) 17 | { 18 | return KDL::Rotation(lhs(0, 0) - rhs(0, 0), lhs(0, 1) - rhs(0, 1), lhs(0, 2) - rhs(0, 2), 19 | lhs(1, 0) - rhs(1, 0), lhs(1, 1) - rhs(1, 1), lhs(1, 2) - rhs(1, 2), 20 | lhs(2, 0) - rhs(2, 0), lhs(2, 1) - rhs(2, 1), lhs(2, 2) - rhs(2, 2)); 21 | } 22 | } 23 | 24 | // ----------------------------------------------------------------------------- 25 | 26 | MatrixExponential::MatrixExponential(motion _motionType, const KDL::Vector & _axis, const KDL::Vector & _origin) 27 | : motionType(_motionType), 28 | axis(_axis), 29 | origin(_origin) 30 | { 31 | axis.Normalize(); 32 | } 33 | 34 | // ----------------------------------------------------------------------------- 35 | 36 | KDL::Frame MatrixExponential::asFrame(double theta) const 37 | { 38 | KDL::Frame H; 39 | 40 | switch (motionType) 41 | { 42 | case ROTATION: 43 | H.M = KDL::Rotation::Rot2(axis, theta); 44 | H.p = (KDL::Rotation::Identity() - H.M) * (axis * origin * axis); 45 | break; 46 | case TRANSLATION: 47 | H.p = axis * theta; 48 | break; 49 | default: 50 | yCWarning(ST) << "Unrecognized motion type:" << motionType; 51 | } 52 | 53 | return H; 54 | } 55 | 56 | // ----------------------------------------------------------------------------- 57 | 58 | void MatrixExponential::changeBase(const KDL::Frame & H_new_old) 59 | { 60 | axis = H_new_old.M * axis; 61 | 62 | if (motionType == ROTATION) 63 | { 64 | origin = H_new_old * origin; 65 | } 66 | } 67 | 68 | // ----------------------------------------------------------------------------- 69 | 70 | MatrixExponential MatrixExponential::cloneWithBase(const KDL::Frame & H_new_old) const 71 | { 72 | MatrixExponential exp(*this); 73 | exp.changeBase(H_new_old); 74 | return exp; 75 | } 76 | 77 | // ----------------------------------------------------------------------------- 78 | -------------------------------------------------------------------------------- /libraries/ScrewTheoryLib/MatrixExponential.hpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #ifndef __MATRIX_EXPONENTIAL_HPP__ 4 | #define __MATRIX_EXPONENTIAL_HPP__ 5 | 6 | #include 7 | 8 | namespace roboticslab 9 | { 10 | 11 | /** 12 | * @ingroup ScrewTheoryLib 13 | * 14 | * @brief Abstraction of a term in a product of exponentials (POE) formula. 15 | * 16 | * @see PoeExpression 17 | */ 18 | class MatrixExponential 19 | { 20 | public: 21 | //! Lists available screw motion types. 22 | enum motion 23 | { 24 | ROTATION, ///< Revolute joint (zero-pitch twist). 25 | TRANSLATION ///< Prismatic joint (infinite-pitch twist). 26 | }; 27 | 28 | /** 29 | * @brief Constructor 30 | * 31 | * @param motionType Screw motion type as defined in \ref motion. 32 | * @param axis Screw axis. 33 | * @param origin A point along the screw axis (defaults to (0, 0, 0), ignored in 34 | * prismatic joints). 35 | */ 36 | MatrixExponential(motion motionType, const KDL::Vector & axis, const KDL::Vector & origin = KDL::Vector::Zero()); 37 | 38 | /** 39 | * @brief Evaluates this term for the given magnitude of the screw 40 | * 41 | * @param theta Input magnitude this screw should be computed at. 42 | * 43 | * @return Resulting homogeneous transformation matrix. 44 | */ 45 | KDL::Frame asFrame(double theta) const; 46 | 47 | /** 48 | * @brief Retrieves the \ref motion type of this screw 49 | * 50 | * @return A \ref motion type this term has been registered with. 51 | */ 52 | motion getMotionType() const 53 | { return motionType; } 54 | 55 | /** 56 | * @brief Screw axis 57 | * 58 | * @return A vector representing the direction of the screw axis. 59 | */ 60 | const KDL::Vector & getAxis() const 61 | { return axis; } 62 | 63 | /** 64 | * @brief A point along the screw axis 65 | * 66 | * @return A vector representing the position of some point along the screw axis. 67 | */ 68 | const KDL::Vector & getOrigin() const 69 | { return origin; } 70 | 71 | /** 72 | * @brief Refers the internal coordinates of this screw to a different base 73 | * 74 | * @param H_new_old Transformation between the new and the old base frame. 75 | * 76 | * @see cloneWithBase 77 | */ 78 | void changeBase(const KDL::Frame & H_new_old); 79 | 80 | /** 81 | * @brief Clones this instance and refers the internal coordinates of the screw to a different base 82 | * 83 | * @param H_new_old Transformation between the new and the old base frame. 84 | * 85 | * @return An instance referred to the new frame. 86 | * 87 | * @see changeBase 88 | */ 89 | MatrixExponential cloneWithBase(const KDL::Frame & H_new_old) const; 90 | 91 | private: 92 | motion motionType; 93 | KDL::Vector axis; 94 | KDL::Vector origin; 95 | }; 96 | 97 | } // namespace roboticslab 98 | 99 | #endif // __MATRIX_EXPONENTIAL_HPP__ 100 | -------------------------------------------------------------------------------- /libraries/ScrewTheoryLib/ScrewTheoryTools.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include "ScrewTheoryTools.hpp" 4 | 5 | #include 6 | 7 | #include 8 | 9 | // ----------------------------------------------------------------------------- 10 | 11 | KDL::Rotation roboticslab::vectorPow2(const KDL::Vector & v) 12 | { 13 | return KDL::Rotation(v.x() * v.x(), v.x() * v.y(), v.x() * v.z(), 14 | v.x() * v.y(), v.y() * v.y(), v.y() * v.z(), 15 | v.x() * v.z(), v.y() * v.z(), v.z() * v.z()); 16 | } 17 | 18 | // ----------------------------------------------------------------------------- 19 | 20 | double roboticslab::normalizeAngle(double angle) 21 | { 22 | if (KDL::Equal(std::abs(angle), KDL::PI)) 23 | { 24 | return KDL::PI; 25 | } 26 | else if (angle > KDL::PI) 27 | { 28 | return angle - 2 * KDL::PI; 29 | } 30 | else if (angle < -KDL::PI) 31 | { 32 | return angle + 2 * KDL::PI; 33 | } 34 | else 35 | { 36 | return angle; 37 | } 38 | } 39 | 40 | // ----------------------------------------------------------------------------- 41 | -------------------------------------------------------------------------------- /libraries/ScrewTheoryLib/ScrewTheoryTools.hpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #ifndef __SCREW_THEORY_TOOLS_HPP__ 4 | #define __SCREW_THEORY_TOOLS_HPP__ 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | namespace roboticslab 12 | { 13 | 14 | /** 15 | * @ingroup kinematics-dynamics-libraries 16 | * \defgroup ScrewTheoryLib 17 | * 18 | * @brief Contains classes related to Screw Theory solvers and tools. 19 | * 20 | * Proof of concept for a kinematics library based on 21 | * screw theory 22 | * concepts applied to robotics. This implementation is mainly focused 23 | * on solving inverse kinematics problems in an efficient and effective 24 | * manner via closed-form geometric solutions. Comparing this approach 25 | * with a tradicional numeric-based approach yields 26 | * (@cite pardosgotor2018str @cite pardosgotor2022str): 27 | * - faster solutions, since there are no iterations involved; 28 | * - exact solutions, since the direct formulation guarantees convergence; 29 | * - multiple solutions and the possibility to choose the better ones. 30 | * 31 | * @see @cite lukawski2022jjaa 32 | */ 33 | 34 | /** 35 | * @ingroup ScrewTheoryLib 36 | * 37 | * @brief Multiply a vector by itself to obtain a square matrix 38 | * 39 | * Given a column-vector v, computes v*v'. 40 | * 41 | * @param v Input vector. 42 | * 43 | * @return Resulting square matrix. 44 | */ 45 | KDL::Rotation vectorPow2(const KDL::Vector & v); 46 | 47 | /** 48 | * @ingroup ScrewTheoryLib 49 | * 50 | * @brief Clip an angle value between certain bounds. 51 | * 52 | * Values that exceed [-pi pi] are normalized to fit this range. Values 53 | * close to -pi are approximated as pi. 54 | * 55 | * @param angle Magnitude of the requested angle. 56 | * 57 | * @return Normalized angle. 58 | */ 59 | double normalizeAngle(double angle); 60 | 61 | } // namespace roboticslab 62 | 63 | #endif // __SCREW_THEORY_TOOLS_HPP__ 64 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/AsibotSolver/AsibotConfigurationLeastOverallAngularDisplacement.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include "AsibotConfiguration.hpp" 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include "LogComponent.hpp" 13 | 14 | using namespace roboticslab; 15 | 16 | bool AsibotConfigurationLeastOverallAngularDisplacement::findOptimalConfiguration(JointsIn qGuess) 17 | { 18 | using MapPair = std::pair; 19 | using MapType = std::map; 20 | MapType angularDisplacementPerConfiguration; 21 | 22 | if (forwardElbowUp.valid) 23 | { 24 | std::vector diffs = getDiffs(qGuess, forwardElbowUp); 25 | double sum = std::accumulate(diffs.begin(), diffs.end(), 0.0); 26 | angularDisplacementPerConfiguration.insert(MapPair(sum, &forwardElbowUp)); 27 | } 28 | 29 | if (forwardElbowDown.valid) 30 | { 31 | std::vector diffs = getDiffs(qGuess, forwardElbowDown); 32 | double sum = std::accumulate(diffs.begin(), diffs.end(), 0.0); 33 | angularDisplacementPerConfiguration.insert(MapPair(sum, &forwardElbowDown)); 34 | } 35 | 36 | if (reversedElbowUp.valid) 37 | { 38 | std::vector diffs = getDiffs(qGuess, reversedElbowUp); 39 | double sum = std::accumulate(diffs.begin(), diffs.end(), 0.0); 40 | angularDisplacementPerConfiguration.insert(MapPair(sum, &reversedElbowUp)); 41 | } 42 | 43 | if (reversedElbowDown.valid) 44 | { 45 | std::vector diffs = getDiffs(qGuess, reversedElbowDown); 46 | double sum = std::accumulate(diffs.begin(), diffs.end(), 0.0); 47 | angularDisplacementPerConfiguration.insert(MapPair(sum, &reversedElbowDown)); 48 | } 49 | 50 | if (angularDisplacementPerConfiguration.empty()) 51 | { 52 | yCWarning(ASIBOT) << "No valid configuration found"; 53 | return false; 54 | } 55 | 56 | // std::map keys are sorted, pick std::pair with lowest key (angle sum) 57 | MapType::iterator it = angularDisplacementPerConfiguration.begin(); 58 | optimalPose = *it->second; 59 | 60 | // compare with second best option if available 61 | if (angularDisplacementPerConfiguration.size() > 1) 62 | { 63 | Pose alternativePose = *(++it)->second; 64 | 65 | // both candidates share same orientation of joint 1 66 | if (alternativePose._orient == optimalPose._orient) 67 | { 68 | std::vector diffsA = getDiffs(qGuess, optimalPose); 69 | std::vector diffsB = getDiffs(qGuess, alternativePose); 70 | 71 | // alternative pose minimizes angular travel distance for joint 2; 72 | // if equal, prefer elbow up configuration 73 | if (diffsB[1] < diffsA[1] || (diffsB[1] == diffsA[1] && alternativePose._elb == Pose::UP)) 74 | { 75 | optimalPose = alternativePose; 76 | } 77 | } 78 | } 79 | 80 | yCInfo(ASIBOT) << "Using config:" << optimalPose.toString(); 81 | 82 | return true; 83 | } 84 | 85 | std::vector AsibotConfigurationLeastOverallAngularDisplacement::getDiffs(JointsIn qGuess, const Pose & pose) 86 | { 87 | return { 88 | std::abs(qGuess[0] - pose._q1), 89 | std::abs(qGuess[1] - pose._q2), 90 | std::abs(qGuess[2] - pose._q3), 91 | std::abs(qGuess[3] - pose._q4), 92 | std::abs(qGuess[4] - pose._q5) 93 | }; 94 | } 95 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/AsibotSolver/AsibotSolver.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include "AsibotSolver.hpp" 4 | 5 | using namespace roboticslab; 6 | 7 | // ----------------------------------------------------------------------------- 8 | 9 | AsibotConfiguration * AsibotSolver::getConfiguration() const 10 | { 11 | return confFactory->create(); 12 | } 13 | 14 | // ----------------------------------------------------------------------------- 15 | 16 | AsibotSolver::AsibotTcpFrame AsibotSolver::getTcpFrame() const 17 | { 18 | std::lock_guard lock(mtx); 19 | return tcpFrameStruct; 20 | } 21 | 22 | // ----------------------------------------------------------------------------- 23 | 24 | void AsibotSolver::setTcpFrame(const AsibotTcpFrame & tcpFrameStruct) 25 | { 26 | std::lock_guard lock(mtx); 27 | this->tcpFrameStruct = tcpFrameStruct; 28 | } 29 | 30 | // ----------------------------------------------------------------------------- 31 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/AsibotSolver/AsibotSolver.hpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #ifndef __ASIBOT_SOLVER_HPP__ 4 | #define __ASIBOT_SOLVER_HPP__ 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "AsibotConfiguration.hpp" 13 | #include "ICartesianSolver.h" 14 | #include "AsibotSolver_ParamsParser.h" 15 | 16 | /** 17 | * @ingroup YarpPlugins 18 | * @defgroup AsibotSolver 19 | * 20 | * @brief Contains AsibotSolver. 21 | */ 22 | 23 | /** 24 | * @ingroup AsibotSolver 25 | * @brief The AsibotSolver implements ICartesianSolver. 26 | */ 27 | class AsibotSolver : public yarp::dev::DeviceDriver, 28 | public roboticslab::ICartesianSolver, 29 | public AsibotSolver_ParamsParser 30 | { 31 | public: 32 | // -------- ICartesianSolver declarations. Implementation in ICartesianSolverImpl.cpp -------- 33 | 34 | // Get number of joints for which the solver has been configured. 35 | int getNumJoints() override; 36 | 37 | // Get number of TCPs for which the solver has been configured. 38 | int getNumTcps() override; 39 | 40 | // Append an additional link. 41 | bool appendLink(const std::vector &x) override; 42 | 43 | // Restore original kinematic chain. 44 | bool restoreOriginalChain() override; 45 | 46 | // Change reference frame. 47 | bool changeOrigin(const std::vector &x_old_obj, const std::vector &x_new_old, std::vector &x_new_obj) override; 48 | 49 | // Perform forward kinematics. 50 | bool fwdKin(const std::vector &q, std::vector &x) override; 51 | 52 | // Obtain difference between supplied pose inputs. 53 | bool poseDiff(const std::vector &xLhs, const std::vector &xRhs, std::vector &xOut) override; 54 | 55 | // Perform inverse kinematics. 56 | bool invKin(const std::vector &xd, const std::vector &qGuess, std::vector &q, reference_frame frame) override; 57 | 58 | // Perform differential inverse kinematics. 59 | bool diffInvKin(const std::vector &q, const std::vector &xdot, std::vector &qdot, reference_frame frame) override; 60 | 61 | // Perform inverse dynamics. 62 | bool invDyn(const std::vector &q, std::vector &t) override; 63 | 64 | // Perform inverse dynamics. 65 | bool invDyn(const std::vector &q, const std::vector &qdot, const std::vector &qdotdot, 66 | const std::vector &ftip, std::vector &t, reference_frame frame) override; 67 | 68 | // -------- DeviceDriver declarations. Implementation in DeviceDriverImpl.cpp -------- 69 | bool open(yarp::os::Searchable& config) override; 70 | bool close() override; 71 | 72 | private: 73 | struct AsibotTcpFrame 74 | { 75 | bool hasFrame; 76 | yarp::sig::Matrix frameTcp; 77 | }; 78 | 79 | roboticslab::AsibotConfiguration * getConfiguration() const; 80 | 81 | AsibotTcpFrame getTcpFrame() const; 82 | void setTcpFrame(const AsibotTcpFrame & tcpFrameStruct); 83 | 84 | roboticslab::AsibotConfigurationFactory * confFactory {nullptr}; 85 | 86 | AsibotTcpFrame tcpFrameStruct; 87 | 88 | mutable std::mutex mtx; 89 | }; 90 | 91 | #endif // __ASIBOT_SOLVER_HPP__ 92 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/AsibotSolver/AsibotSolver_params.md: -------------------------------------------------------------------------------- 1 | | Group | Parameter | Type | Units | Default Value | Required | Description | Notes | 2 | |:-----:|:--------------:|:--------------:|:-----:|:-------------------------------:|:--------:|:---------------------------------:|:-----:| 3 | | | A0 | double | m | 0.3 | no | length of link 1 | | 4 | | | A1 | double | m | 0.4 | no | length of link 2 | | 5 | | | A2 | double | m | 0.4 | no | length of link 3 | | 6 | | | A3 | double | m | 0.3 | no | length of link 4 | | 7 | | | invKinStrategy | string | | leastOverallAngularDisplacement | no | IK configuration strategy | | 8 | | | mins | vector | deg | | yes | lower bound joint position limits | | 9 | | | maxs | vector | deg | | yes | upper bound joint position limits | | 10 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/AsibotSolver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(NOT YARP_math_FOUND AND (NOT DEFINED ENABLE_AsibotSolver OR ENABLE_AsibotSolver)) 2 | message(WARNING "YARP_math lib not found, disabling AsibotSolver") 3 | endif() 4 | 5 | yarp_prepare_plugin(AsibotSolver 6 | CATEGORY device 7 | TYPE AsibotSolver 8 | INCLUDE AsibotSolver.hpp 9 | DEFAULT ON 10 | DEPENDS "YARP_math_FOUND;ENABLE_KinematicRepresentationLib" 11 | GENERATE_PARSER) 12 | 13 | if(NOT SKIP_AsibotSolver) 14 | 15 | yarp_add_plugin(AsibotSolver) 16 | 17 | target_sources(AsibotSolver PRIVATE AsibotSolver.hpp 18 | AsibotSolver.cpp 19 | DeviceDriverImpl.cpp 20 | ICartesianSolverImpl.cpp 21 | AsibotConfiguration.hpp 22 | AsibotConfiguration.cpp 23 | AsibotConfigurationLeastOverallAngularDisplacement.cpp 24 | AsibotSolver_ParamsParser.h 25 | AsibotSolver_ParamsParser.cpp 26 | LogComponent.hpp 27 | LogComponent.cpp) 28 | 29 | target_link_libraries(AsibotSolver YARP::YARP_os 30 | YARP::YARP_dev 31 | YARP::YARP_sig 32 | YARP::YARP_math 33 | ROBOTICSLAB::KinematicRepresentationLib 34 | ROBOTICSLAB::KinematicsDynamicsInterfaces) 35 | 36 | yarp_install(TARGETS AsibotSolver 37 | LIBRARY DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_DYNAMIC_PLUGINS_INSTALL_DIR} 38 | ARCHIVE DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_STATIC_PLUGINS_INSTALL_DIR} 39 | YARP_INI DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_PLUGIN_MANIFESTS_INSTALL_DIR}) 40 | 41 | else() 42 | 43 | set(ENABLE_AsibotSolver OFF CACHE BOOL "Enable/disable AsibotSolver device" FORCE) 44 | 45 | endif() 46 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/AsibotSolver/DeviceDriverImpl.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include "AsibotSolver.hpp" 4 | 5 | #include 6 | 7 | #include 8 | 9 | #include "LogComponent.hpp" 10 | 11 | using namespace roboticslab; 12 | 13 | constexpr auto NUM_MOTORS = 5; 14 | 15 | // ------------------- DeviceDriver Related ------------------------------------ 16 | 17 | bool AsibotSolver::open(yarp::os::Searchable& config) 18 | { 19 | if (!parseParams(config)) 20 | { 21 | yCError(ASIBOT) << "Failed to parse parameters"; 22 | return false; 23 | } 24 | 25 | if (m_mins.size() != NUM_MOTORS || m_maxs.size() != NUM_MOTORS) 26 | { 27 | yCError(ASIBOT) << "Vectors of joints limits must have exactly" << NUM_MOTORS << "elements"; 28 | return false; 29 | } 30 | 31 | if (m_invKinStrategy == m_invKinStrategy_defaultValue) 32 | { 33 | confFactory = new AsibotConfigurationLeastOverallAngularDisplacementFactory(m_mins, m_maxs); 34 | } 35 | else 36 | { 37 | yCError(ASIBOT) << "Unsupported IK configuration strategy:" << m_invKinStrategy; 38 | return false; 39 | } 40 | 41 | tcpFrameStruct.hasFrame = false; 42 | tcpFrameStruct.frameTcp = yarp::math::eye(4); 43 | 44 | return true; 45 | } 46 | 47 | // ----------------------------------------------------------------------------- 48 | 49 | bool AsibotSolver::close() 50 | { 51 | if (confFactory) 52 | { 53 | delete confFactory; 54 | confFactory = nullptr; 55 | } 56 | 57 | return true; 58 | } 59 | 60 | // ----------------------------------------------------------------------------- 61 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/AsibotSolver/LogComponent.cpp: -------------------------------------------------------------------------------- 1 | #include "LogComponent.hpp" 2 | 3 | YARP_LOG_COMPONENT(ASIBOT, "rl.AsibotSolver") 4 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/AsibotSolver/LogComponent.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __ASIBOT_SOLVER_LOG_COMPONENT_HPP__ 2 | #define __ASIBOT_SOLVER_LOG_COMPONENT_HPP__ 3 | 4 | #include 5 | 6 | YARP_DECLARE_LOG_COMPONENT(ASIBOT) 7 | 8 | #endif // __ASIBOT_SOLVER_LOG_COMPONENT_HPP__ 9 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl_params.md: -------------------------------------------------------------------------------- 1 | | Group | Parameter | Type | Units | Default Value | Required | Description | Notes | 2 | |:-----:|:------------------:|:------:|:-----:|:-------------------:|:--------:|:-------------------------------------------:|:---------:| 3 | | | controllerGain | double | | 0.05 | no | controller gain | | 4 | | | trajectoryDuration | double | s | 10.0 | no | trajectory duration | | 5 | | | cmcPeriodMs | int | ms | 50 | no | CMC rate | | 6 | | | waitPeriodMs | int | ms | 30 | no | wait command period | | 7 | | | usePosdMovl | bool | | false | no | execute MOVL commands in POSD mode using IK | | 8 | | | enableFailFast | bool | | false | no | enable fail-fast mode for MOVL commands | | 9 | | | referenceFrame | string | | base | no | reference frame | base, tcp | 10 | | | robot | string | | remote_controlboard | no | robot device | | 11 | | | solver | string | | KdlSolver | no | cartesian solver device | | 12 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/BasicCartesianControl/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | yarp_prepare_plugin(BasicCartesianControl 2 | CATEGORY device 3 | TYPE BasicCartesianControl 4 | INCLUDE BasicCartesianControl.hpp 5 | DEFAULT ON 6 | DEPENDS "ENABLE_KdlVectorConverterLib;orocos_kdl_FOUND" 7 | EXTRA_CONFIG WRAPPER=CartesianControlServer 8 | GENERATE_PARSER) 9 | 10 | if(NOT SKIP_BasicCartesianControl) 11 | 12 | yarp_add_plugin(BasicCartesianControl) 13 | 14 | target_sources(BasicCartesianControl PRIVATE BasicCartesianControl.hpp 15 | BasicCartesianControl.cpp 16 | DeviceDriverImpl.cpp 17 | ICartesianControlImpl.cpp 18 | PeriodicThreadImpl.cpp 19 | BasicCartesianControl_ParamsParser.h 20 | BasicCartesianControl_ParamsParser.cpp 21 | LogComponent.hpp 22 | LogComponent.cpp) 23 | 24 | target_link_libraries(BasicCartesianControl YARP::YARP_os 25 | YARP::YARP_dev 26 | ${orocos_kdl_LIBRARIES} 27 | ROBOTICSLAB::KdlVectorConverterLib 28 | ROBOTICSLAB::KinematicsDynamicsInterfaces) 29 | 30 | target_include_directories(BasicCartesianControl PRIVATE ${orocos_kdl_INCLUDE_DIRS}) 31 | 32 | yarp_install(TARGETS BasicCartesianControl 33 | LIBRARY DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_DYNAMIC_PLUGINS_INSTALL_DIR} 34 | ARCHIVE DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_STATIC_PLUGINS_INSTALL_DIR} 35 | YARP_INI DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_PLUGIN_MANIFESTS_INSTALL_DIR}) 36 | 37 | endif() 38 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/BasicCartesianControl/LogComponent.cpp: -------------------------------------------------------------------------------- 1 | #include "LogComponent.hpp" 2 | 3 | YARP_LOG_COMPONENT(BCC, "rl.BasicCartesianControl") 4 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/BasicCartesianControl/LogComponent.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __BASIC_CARTESIAN_CONTROL_LOG_COMPONENT_HPP__ 2 | #define __BASIC_CARTESIAN_CONTROL_LOG_COMPONENT_HPP__ 3 | 4 | #include 5 | 6 | YARP_DECLARE_LOG_COMPONENT(BCC) 7 | 8 | #endif // __BASIC_CARTESIAN_CONTROL_LOG_COMPONENT_HPP__ 9 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Create device interface target. 2 | add_library(KinematicsDynamicsInterfaces INTERFACE) 3 | 4 | # Configure usage requirements. 5 | target_link_libraries(KinematicsDynamicsInterfaces INTERFACE YARP::YARP_os) 6 | 7 | target_include_directories(KinematicsDynamicsInterfaces INTERFACE $ 8 | $) 9 | 10 | # Register interface headers. 11 | set_property(TARGET KinematicsDynamicsInterfaces PROPERTY PUBLIC_HEADER ICartesianControl.h 12 | ICartesianSolver.h) 13 | 14 | # Register export set. 15 | install(TARGETS KinematicsDynamicsInterfaces 16 | EXPORT ROBOTICSLAB_KINEMATICS_DYNAMICS) 17 | 18 | # Create alias target. 19 | add_library(ROBOTICSLAB::KinematicsDynamicsInterfaces ALIAS KinematicsDynamicsInterfaces) 20 | 21 | # Create devices. 22 | add_subdirectory(AsibotSolver) 23 | add_subdirectory(BasicCartesianControl) 24 | add_subdirectory(CartesianControlClient) 25 | add_subdirectory(CartesianControlServer) 26 | add_subdirectory(CartesianControlServerROS2) 27 | add_subdirectory(KdlSolver) 28 | add_subdirectory(KdlTreeSolver) 29 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlClient/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | yarp_prepare_plugin(CartesianControlClient 2 | CATEGORY device 3 | TYPE CartesianControlClient 4 | INCLUDE CartesianControlClient.hpp 5 | DEFAULT ON 6 | GENERATE_PARSER) 7 | 8 | if(NOT SKIP_CartesianControlClient) 9 | 10 | yarp_add_plugin(CartesianControlClient) 11 | 12 | target_sources(CartesianControlClient PRIVATE CartesianControlClient.hpp 13 | DeviceDriverImpl.cpp 14 | ICartesianControlImpl.cpp 15 | FkStreamResponder.cpp 16 | CartesianControlClient_ParamsParser.h 17 | CartesianControlClient_ParamsParser.cpp 18 | LogComponent.hpp 19 | LogComponent.cpp) 20 | 21 | target_link_libraries(CartesianControlClient YARP::YARP_os 22 | YARP::YARP_dev 23 | ROBOTICSLAB::KinematicsDynamicsInterfaces) 24 | 25 | yarp_install(TARGETS CartesianControlClient 26 | LIBRARY DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_DYNAMIC_PLUGINS_INSTALL_DIR} 27 | ARCHIVE DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_STATIC_PLUGINS_INSTALL_DIR} 28 | YARP_INI DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_PLUGIN_MANIFESTS_INSTALL_DIR}) 29 | 30 | endif() 31 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #ifndef __CARTESIAN_CONTROL_CLIENT_HPP__ 4 | #define __CARTESIAN_CONTROL_CLIENT_HPP__ 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include "ICartesianControl.h" 17 | #include "CartesianControlClient_ParamsParser.h" 18 | 19 | /** 20 | * @ingroup YarpPlugins 21 | * @defgroup CartesianControlClient 22 | * 23 | * @brief Contains CartesianControlClient. 24 | */ 25 | 26 | /** 27 | * @ingroup CartesianControlClient 28 | * @brief Responds to streaming FK messages. 29 | */ 30 | class FkStreamResponder : public yarp::os::TypedReaderCallback 31 | { 32 | public: 33 | FkStreamResponder(); 34 | void onRead(yarp::os::Bottle& b) override; 35 | bool getLastStatData(std::vector &x, int *state, double * timestamp, double timeout); 36 | 37 | private: 38 | double localArrivalTime; 39 | int state; 40 | double timestamp; 41 | std::vector x; 42 | mutable std::mutex mtx; 43 | }; 44 | 45 | /** 46 | * @ingroup CartesianControlClient 47 | * @brief The CartesianControlClient class implements ICartesianControl client side. 48 | */ 49 | class CartesianControlClient : public yarp::dev::DeviceDriver, 50 | public roboticslab::ICartesianControl, 51 | public CartesianControlClient_ParamsParser 52 | { 53 | public: 54 | // -- ICartesianControl declarations. Implementation in ICartesianControlImpl.cpp-- 55 | bool stat(std::vector &x, int * state = nullptr, double * timestamp = nullptr) override; 56 | bool inv(const std::vector &xd, std::vector &q) override; 57 | bool movj(const std::vector &xd) override; 58 | bool relj(const std::vector &xd) override; 59 | bool movl(const std::vector &xd) override; 60 | bool movv(const std::vector &xdotd) override; 61 | bool gcmp() override; 62 | bool forc(const std::vector &fd) override; 63 | bool stopControl() override; 64 | bool wait(double timeout) override; 65 | bool tool(const std::vector &x) override; 66 | bool act(int command) override; 67 | void pose(const std::vector &x) override; 68 | void twist(const std::vector &xdot) override; 69 | void wrench(const std::vector &w) override; 70 | bool setParameter(int vocab, double value) override; 71 | bool getParameter(int vocab, double * value) override; 72 | bool setParameters(const std::map & params) override; 73 | bool getParameters(std::map & params) override; 74 | 75 | // -------- DeviceDriver declarations. Implementation in IDeviceImpl.cpp -------- 76 | bool open(yarp::os::Searchable& config) override; 77 | bool close() override; 78 | 79 | private: 80 | bool handleRpcRunnableCmd(int vocab); 81 | bool handleRpcConsumerCmd(int vocab, const std::vector& in); 82 | bool handleRpcFunctionCmd(int vocab, const std::vector& in, std::vector& out); 83 | 84 | void handleStreamingConsumerCmd(int vocab, const std::vector& in); 85 | void handleStreamingBiConsumerCmd(int vocab, const std::vector& in1, double in2); 86 | 87 | yarp::os::RpcClient rpcClient; 88 | yarp::os::BufferedPort fkInPort, commandPort; 89 | 90 | FkStreamResponder fkStreamResponder; 91 | }; 92 | 93 | #endif // __CARTESIAN_CONTROL_CLIENT_HPP__ 94 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlClient/CartesianControlClient_ParamsParser.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SPDX-FileCopyrightText: 2023-2023 Istituto Italiano di Tecnologia (IIT) 3 | * SPDX-License-Identifier: LGPL-2.1-or-later 4 | */ 5 | 6 | 7 | // Generated by yarpDeviceParamParserGenerator (1.0) 8 | // This is an automatically generated file. Please do not edit it. 9 | // It will be re-generated if the cmake flag ALLOW_DEVICE_PARAM_PARSER_GERNERATION is ON. 10 | 11 | // Generated on: Wed Apr 16 14:50:07 2025 12 | 13 | 14 | #ifndef CARTESIANCONTROLCLIENT_PARAMSPARSER_H 15 | #define CARTESIANCONTROLCLIENT_PARAMSPARSER_H 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | /** 23 | * This class is the parameters parser for class CartesianControlClient. 24 | * 25 | * These are the used parameters: 26 | * | Group name | Parameter name | Type | Units | Default Value | Required | Description | Notes | 27 | * |:----------:|:-------------------:|:------:|:-----:|:-----------------------:|:--------:|:-----------------:|:-----:| 28 | * | - | cartesianLocal | string | - | /CartesianControlClient | 0 | local port | - | 29 | * | - | cartesianRemote | string | - | /CartesianControl | 0 | remote port | - | 30 | * | - | fkStreamTimeoutSecs | double | s | 0.5 | 0 | FK stream timeout | - | 31 | * 32 | * The device can be launched by yarpdev using one of the following examples (with and without all optional parameters): 33 | * \code{.unparsed} 34 | * yarpdev --device CartesianControlClient --cartesianLocal /CartesianControlClient --cartesianRemote /CartesianControl --fkStreamTimeoutSecs 0.5 35 | * \endcode 36 | * 37 | * \code{.unparsed} 38 | * yarpdev --device CartesianControlClient 39 | * \endcode 40 | * 41 | */ 42 | 43 | class CartesianControlClient_ParamsParser : public yarp::dev::IDeviceDriverParams 44 | { 45 | public: 46 | CartesianControlClient_ParamsParser(); 47 | ~CartesianControlClient_ParamsParser() override = default; 48 | 49 | public: 50 | const std::string m_device_classname = {"CartesianControlClient"}; 51 | const std::string m_device_name = {"CartesianControlClient"}; 52 | bool m_parser_is_strict = false; 53 | struct parser_version_type 54 | { 55 | int major = 1; 56 | int minor = 0; 57 | }; 58 | const parser_version_type m_parser_version = {}; 59 | 60 | const std::string m_cartesianLocal_defaultValue = {"/CartesianControlClient"}; 61 | const std::string m_cartesianRemote_defaultValue = {"/CartesianControl"}; 62 | const std::string m_fkStreamTimeoutSecs_defaultValue = {"0.5"}; 63 | 64 | std::string m_cartesianLocal = {"/CartesianControlClient"}; 65 | std::string m_cartesianRemote = {"/CartesianControl"}; 66 | double m_fkStreamTimeoutSecs = {0.5}; 67 | 68 | bool parseParams(const yarp::os::Searchable & config) override; 69 | std::string getDeviceClassName() const override { return m_device_classname; } 70 | std::string getDeviceName() const override { return m_device_name; } 71 | std::string getDocumentationOfDeviceParams() const override; 72 | std::vector getListOfParams() const override; 73 | }; 74 | 75 | #endif 76 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlClient/CartesianControlClient_params.md: -------------------------------------------------------------------------------- 1 | | Group | Parameter | Type | Units | Default Value | Required | Description | Notes | 2 | |:-----:|:-------------------:|:------:|:-----:|:-----------------------:|:--------:|:-----------------:|:-----:| 3 | | | cartesianLocal | string | | /CartesianControlClient | no | local port | | 4 | | | cartesianRemote | string | | /CartesianControl | no | remote port | | 5 | | | fkStreamTimeoutSecs | double | s | 0.5 | no | FK stream timeout | | 6 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlClient/DeviceDriverImpl.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include "CartesianControlClient.hpp" 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include "LogComponent.hpp" 12 | 13 | using namespace roboticslab; 14 | 15 | // ------------------- DeviceDriver Related ------------------------------------ 16 | 17 | bool CartesianControlClient::open(yarp::os::Searchable& config) 18 | { 19 | if (!parseParams(config)) 20 | { 21 | yCError(CCC) << "Failed to parse parameters"; 22 | return false; 23 | } 24 | 25 | if (!rpcClient.open(m_cartesianLocal+ "/rpc:c") || !commandPort.open(m_cartesianLocal + "/command:o")) 26 | { 27 | yCError(CCC) << "Unable to open local RPC and command ports"; 28 | return false; 29 | } 30 | 31 | if (!rpcClient.addOutput(m_cartesianRemote + "/rpc:s")) 32 | { 33 | yCError(CCC) << "Error on connect to remote RPC server"; 34 | return false; 35 | } 36 | 37 | if (!commandPort.addOutput(m_cartesianRemote + "/command:i", "udp")) 38 | { 39 | yCError(CCC) << "Error on connect to remote command server"; 40 | return false; 41 | } 42 | 43 | std::string statePort = m_cartesianRemote + "/state:o"; 44 | 45 | if (yarp::os::Network::exists(statePort)) 46 | { 47 | if (!fkInPort.open(m_cartesianLocal + "/state:i")) 48 | { 49 | yCError(CCC) << "Unable to open local stream port"; 50 | return false; 51 | } 52 | 53 | if (!yarp::os::Network::connect(statePort, fkInPort.getName(), "udp")) 54 | { 55 | yCError(CCC) << "Unable to connect to remote stream port"; 56 | return false; 57 | } 58 | 59 | fkInPort.useCallback(fkStreamResponder); 60 | yarp::os::Time::delay(m_fkStreamTimeoutSecs); // wait for first data to arrive 61 | } 62 | else 63 | { 64 | yCWarning(CCC) << "Missing remote" << statePort << "stream port, using RPC instead"; 65 | } 66 | 67 | yCInfo(CCC) << "Connected to remote" << m_cartesianRemote; 68 | 69 | return true; 70 | } 71 | 72 | // ----------------------------------------------------------------------------- 73 | 74 | bool CartesianControlClient::close() 75 | { 76 | rpcClient.close(); 77 | commandPort.close(); 78 | 79 | if (!fkInPort.isClosed()) 80 | { 81 | fkInPort.close(); 82 | } 83 | 84 | return true; 85 | } 86 | 87 | // ----------------------------------------------------------------------------- 88 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlClient/FkStreamResponder.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include "CartesianControlClient.hpp" 4 | 5 | #include 6 | 7 | using namespace roboticslab; 8 | 9 | // ----------------------------------------------------------------------------- 10 | 11 | FkStreamResponder::FkStreamResponder() 12 | : localArrivalTime(0.0), 13 | state(0), 14 | timestamp(0.0) 15 | {} 16 | 17 | // ----------------------------------------------------------------------------- 18 | 19 | void FkStreamResponder::onRead(yarp::os::Bottle & b) 20 | { 21 | std::lock_guard lock(mtx); 22 | 23 | localArrivalTime = yarp::os::Time::now(); 24 | state = b.get(0).asVocab32(); 25 | x.resize(b.size() - 2); 26 | 27 | for (size_t i = 0; i < x.size(); i++) 28 | { 29 | x[i] = b.get(i + 1).asFloat64(); 30 | } 31 | 32 | timestamp = b.get(b.size() - 1).asFloat64(); 33 | } 34 | 35 | // ----------------------------------------------------------------------------- 36 | 37 | bool FkStreamResponder::getLastStatData(std::vector &x, int *state, double *timestamp, const double timeout) 38 | { 39 | std::lock_guard lock(mtx); 40 | 41 | x = this->x; 42 | 43 | if (state != 0) 44 | { 45 | *state = this->state; 46 | } 47 | 48 | if (timestamp != 0) 49 | { 50 | *timestamp = this->timestamp; 51 | } 52 | 53 | return yarp::os::Time::now() - localArrivalTime <= timeout; 54 | } 55 | 56 | // ----------------------------------------------------------------------------- 57 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlClient/LogComponent.cpp: -------------------------------------------------------------------------------- 1 | #include "LogComponent.hpp" 2 | 3 | YARP_LOG_COMPONENT(CCC, "rl.CartesianControlClient") 4 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlClient/LogComponent.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __CARTESIAN_CONTROL_CLIENT_LOG_COMPONENT_HPP__ 2 | #define __CARTESIAN_CONTROL_CLIENT_LOG_COMPONENT_HPP__ 3 | 4 | #include 5 | 6 | YARP_DECLARE_LOG_COMPONENT(CCC) 7 | 8 | #endif // __CARTESIAN_CONTROL_CLIENT_LOG_COMPONENT_HPP__ 9 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlServer/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | yarp_prepare_plugin(CartesianControlServer 2 | CATEGORY device 3 | TYPE CartesianControlServer 4 | INCLUDE CartesianControlServer.hpp 5 | EXTRA_CONFIG WRAPPER=CartesianControlServer 6 | DEFAULT ON 7 | DEPENDS ENABLE_KinematicRepresentationLib 8 | GENERATE_PARSER) 9 | 10 | if(NOT SKIP_CartesianControlServer) 11 | 12 | yarp_add_plugin(CartesianControlServer) 13 | 14 | target_sources(CartesianControlServer PRIVATE CartesianControlServer.hpp 15 | DeviceDriverImpl.cpp 16 | IWrapperImpl.cpp 17 | PeriodicThreadImpl.cpp 18 | RpcResponder.cpp 19 | StreamResponder.cpp 20 | CartesianControlServer_ParamsParser.h 21 | CartesianControlServer_ParamsParser.cpp 22 | LogComponent.hpp 23 | LogComponent.cpp) 24 | 25 | target_link_libraries(CartesianControlServer YARP::YARP_os 26 | YARP::YARP_dev 27 | ROBOTICSLAB::KinematicRepresentationLib 28 | ROBOTICSLAB::KinematicsDynamicsInterfaces) 29 | 30 | yarp_install(TARGETS CartesianControlServer 31 | LIBRARY DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_DYNAMIC_PLUGINS_INSTALL_DIR} 32 | ARCHIVE DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_STATIC_PLUGINS_INSTALL_DIR} 33 | YARP_INI DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_PLUGIN_MANIFESTS_INSTALL_DIR}) 34 | 35 | endif() 36 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlServer/CartesianControlServer_params.md: -------------------------------------------------------------------------------- 1 | | Group | Parameter | Type | Units | Default Value | Required | Description | Notes | 2 | |:-----:|:------------:|:------:|:-----:|:----------------:|:--------:|:--------------------------------------------:|:----------------------------------------------------------------------:| 3 | | | name | string | | /CartesianServer | no | local port prefix | | 4 | | | fkPeriod | int | ms | 20 | no | FK stream period | | 5 | | | coordRepr | string | | cartesian | no | coordinate representation for transform port | cartesian, cylindrical, spherical, none | 6 | | | angleRepr | string | | axisAngleScaled | no | angle representation for transform port | axisAngle, axisAngleScaled, RPY, eulerYZ, eulerZYZ, polarAzimuth, none | 7 | | | angularUnits | string | | degrees | no | angle representation for transform port | degrees, radians | 8 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlServer/IWrapperImpl.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include "CartesianControlServer.hpp" 4 | 5 | #include 6 | 7 | #include "LogComponent.hpp" 8 | 9 | using namespace roboticslab; 10 | 11 | // ----------------------------------------------------------------------------- 12 | 13 | bool CartesianControlServer::attach(yarp::dev::PolyDriver * poly) 14 | { 15 | if (poly == nullptr) 16 | { 17 | yCError(CCS) << "attach() received nullptr"; 18 | return false; 19 | } 20 | 21 | if (!poly->isValid()) 22 | { 23 | yCError(CCS) << "attach() received invalid PolyDriver"; 24 | return false; 25 | } 26 | 27 | if (!poly->view(iCartesianControl)) 28 | { 29 | yCError(CCS) << "attach() failed to obtain ICartesianControl interface"; 30 | return false; 31 | } 32 | 33 | rpcResponder->setHandle(iCartesianControl); 34 | streamResponder->setHandle(iCartesianControl); 35 | 36 | if (rpcTransformResponder) 37 | { 38 | rpcTransformResponder->setHandle(iCartesianControl); 39 | } 40 | 41 | if (!fkOutPort.isClosed()) 42 | { 43 | return yarp::os::PeriodicThread::start(); 44 | } 45 | 46 | return true; 47 | } 48 | 49 | // ----------------------------------------------------------------------------- 50 | 51 | bool CartesianControlServer::detach() 52 | { 53 | yarp::os::PeriodicThread::stop(); 54 | 55 | rpcResponder->unsetHandle(); 56 | streamResponder->unsetHandle(); 57 | 58 | if (rpcTransformResponder) 59 | { 60 | rpcTransformResponder->unsetHandle(); 61 | } 62 | 63 | iCartesianControl = nullptr; 64 | return true; 65 | } 66 | 67 | // ----------------------------------------------------------------------------- 68 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlServer/LogComponent.cpp: -------------------------------------------------------------------------------- 1 | #include "LogComponent.hpp" 2 | 3 | YARP_LOG_COMPONENT(CCS, "rl.CartesianControlServer") 4 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlServer/LogComponent.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __CARTESIAN_CONTROL_SERVER_LOG_COMPONENT_HPP__ 2 | #define __CARTESIAN_CONTROL_SERVER_LOG_COMPONENT_HPP__ 3 | 4 | #include 5 | 6 | YARP_DECLARE_LOG_COMPONENT(CCS) 7 | 8 | #endif // __CARTESIAN_CONTROL_SERVER_LOG_COMPONENT_HPP__ 9 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlServer/PeriodicThreadImpl.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include "CartesianControlServer.hpp" 4 | 5 | using namespace roboticslab; 6 | 7 | // ------------------- PeriodicThread related ------------------------------------ 8 | 9 | void CartesianControlServer::run() 10 | { 11 | if (iCartesianControl) 12 | { 13 | std::vector x; 14 | int state; 15 | double timestamp; 16 | 17 | if (!iCartesianControl->stat(x, &state, ×tamp)) 18 | { 19 | return; 20 | } 21 | 22 | yarp::os::Bottle & out = fkOutPort.prepare(); 23 | out.clear(); 24 | out.addVocab32(state); 25 | 26 | for (auto i = 0; i < x.size(); i++) 27 | { 28 | out.addFloat64(x[i]); 29 | } 30 | 31 | out.addFloat64(timestamp); 32 | fkOutPort.write(); 33 | } 34 | } 35 | 36 | // ----------------------------------------------------------------------------- 37 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlServer/StreamResponder.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include "CartesianControlServer.hpp" 4 | 5 | #include // std::invoke 6 | 7 | #include 8 | 9 | #include "LogComponent.hpp" 10 | 11 | using namespace roboticslab; 12 | 13 | // ------------------- StreamResponder Related ------------------------------------ 14 | 15 | void StreamResponder::onRead(yarp::os::Bottle& b) 16 | { 17 | yCDebug(CCS, "Got: %s", b.toString().c_str()); 18 | 19 | if (!iCartesianControl) 20 | { 21 | yCError(CCS) << "Invalid ICartesianControl interface"; 22 | return; 23 | } 24 | 25 | switch (b.get(0).asVocab32()) 26 | { 27 | case VOCAB_CC_POSE: 28 | handleConsumerCmdMsg(b, &ICartesianControl::pose); 29 | break; 30 | case VOCAB_CC_TWIST: 31 | handleConsumerCmdMsg(b, &ICartesianControl::twist); 32 | break; 33 | case VOCAB_CC_WRENCH: 34 | handleConsumerCmdMsg(b, &ICartesianControl::wrench); 35 | break; 36 | default: 37 | yCError(CCS) << "Command not recognized:" << b.get(0).toString(); 38 | break; 39 | } 40 | } 41 | 42 | // ----------------------------------------------------------------------------- 43 | 44 | void StreamResponder::handleConsumerCmdMsg(const yarp::os::Bottle& in, ConsumerFun cmd) 45 | { 46 | if (in.size() > 1) 47 | { 48 | std::vector v; 49 | 50 | for (size_t i = 1; i < in.size(); i++) 51 | { 52 | v.push_back(in.get(i).asFloat64()); 53 | } 54 | 55 | std::invoke(cmd, iCartesianControl, v); 56 | } 57 | else 58 | { 59 | yCError(CCS) << "Size error:" << in.size(); 60 | } 61 | } 62 | 63 | // ----------------------------------------------------------------------------- 64 | 65 | void StreamResponder::handleBiConsumerCmdMsg(const yarp::os::Bottle& in, BiConsumerFun cmd) 66 | { 67 | if (in.size() > 2) 68 | { 69 | double d = in.get(1).asFloat64(); 70 | std::vector v; 71 | 72 | for (size_t i = 2; i < in.size(); i++) 73 | { 74 | v.push_back(in.get(i).asFloat64()); 75 | } 76 | 77 | std::invoke(cmd, iCartesianControl, v, d); 78 | } 79 | else 80 | { 81 | yCError(CCS) << "Size error:" << in.size(); 82 | } 83 | } 84 | 85 | // ----------------------------------------------------------------------------- 86 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlServerROS2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(NOT DEFINED ENABLE_CartesianControlServerROS2 OR ENABLE_CartesianControlServerROS2) 2 | if(NOT orocos_kdl_FOUND) 3 | message(WARNING "orocos_kdl package not found, disabling CartesianControlServerROS2") 4 | elseif(NOT rclcpp_FOUND) 5 | message(WARNING "rclcpp package not found, disabling CartesianControlServerROS2") 6 | elseif(NOT std_msgs_FOUND) 7 | message(WARNING "std_msgs package not found, disabling CartesianControlServerROS2") 8 | elseif(NOT std_srvs_FOUND) 9 | message(WARNING "std_srvs package not found, disabling CartesianControlServerROS2") 10 | elseif(NOT geometry_msgs_FOUND) 11 | message(WARNING "geometry_msgs package not found, disabling CartesianControlServerROS2") 12 | endif() 13 | endif() 14 | 15 | yarp_prepare_plugin(CartesianControlServerROS2 16 | CATEGORY device 17 | TYPE CartesianControlServerROS2 18 | INCLUDE CartesianControlServerROS2.hpp 19 | EXTRA_CONFIG WRAPPER=CartesianControlServerROS2 20 | DEFAULT ON 21 | DEPENDS "orocos_kdl_FOUND;rclcpp_FOUND;std_msgs_FOUND;std_srvs_FOUND;geometry_msgs_FOUND" 22 | GENERATE_PARSER) 23 | 24 | if(NOT SKIP_CartesianControlServerROS2) 25 | 26 | yarp_add_plugin(CartesianControlServerROS2) 27 | 28 | target_sources(CartesianControlServerROS2 PRIVATE CartesianControlServerROS2.hpp 29 | CartesianControlServerROS2.cpp 30 | PeriodicThreadImpl.cpp 31 | DeviceDriverImpl.cpp 32 | IWrapperImpl.cpp 33 | Spinner.hpp 34 | Spinner.cpp 35 | CartesianControlServerROS2_ParamsParser.h 36 | CartesianControlServerROS2_ParamsParser.cpp 37 | LogComponent.hpp 38 | LogComponent.cpp) 39 | 40 | target_link_libraries(CartesianControlServerROS2 YARP::YARP_os 41 | YARP::YARP_dev 42 | rclcpp::rclcpp 43 | std_msgs::std_msgs__rosidl_typesupport_cpp 44 | std_srvs::std_srvs__rosidl_typesupport_cpp 45 | geometry_msgs::geometry_msgs__rosidl_typesupport_cpp 46 | ROBOTICSLAB::KinematicsDynamicsInterfaces 47 | ${orocos_kdl_LIBRARIES}) 48 | 49 | target_include_directories(CartesianControlServerROS2 PRIVATE ${orocos_kdl_INCLUDE_DIRS}) 50 | 51 | yarp_install(TARGETS CartesianControlServerROS2 52 | LIBRARY DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_DYNAMIC_PLUGINS_INSTALL_DIR} 53 | ARCHIVE DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_STATIC_PLUGINS_INSTALL_DIR} 54 | YARP_INI DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_PLUGIN_MANIFESTS_INSTALL_DIR}) 55 | 56 | else() 57 | 58 | set(ENABLE_CartesianControlServerROS2 OFF CACHE BOOL "Enable/disable CartesianControlServerROS2 device" FORCE) 59 | 60 | endif() 61 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlServerROS2/CartesianControlServerROS2_ParamsParser.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SPDX-FileCopyrightText: 2023-2023 Istituto Italiano di Tecnologia (IIT) 3 | * SPDX-License-Identifier: LGPL-2.1-or-later 4 | */ 5 | 6 | 7 | // Generated by yarpDeviceParamParserGenerator (1.0) 8 | // This is an automatically generated file. Please do not edit it. 9 | // It will be re-generated if the cmake flag ALLOW_DEVICE_PARAM_PARSER_GERNERATION is ON. 10 | 11 | // Generated on: Wed May 21 16:04:22 2025 12 | 13 | 14 | #ifndef CARTESIANCONTROLSERVERROS2_PARAMSPARSER_H 15 | #define CARTESIANCONTROLSERVERROS2_PARAMSPARSER_H 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | /** 23 | * This class is the parameters parser for class CartesianControlServerROS2. 24 | * 25 | * These are the used parameters: 26 | * | Group name | Parameter name | Type | Units | Default Value | Required | Description | Notes | 27 | * |:----------:|:--------------:|:------:|:-----:|:-----------------------------:|:--------:|:----------------:|:-----:| 28 | * | - | name | string | - | cartesian_control_server_ros2 | 0 | ROS node name | - | 29 | * | - | fkPeriod | int | ms | 20 | 0 | FK stream period | - | 30 | * 31 | * The device can be launched by yarpdev using one of the following examples (with and without all optional parameters): 32 | * \code{.unparsed} 33 | * yarpdev --device CartesianControlServerROS2 --name cartesian_control_server_ros2 --fkPeriod 20 34 | * \endcode 35 | * 36 | * \code{.unparsed} 37 | * yarpdev --device CartesianControlServerROS2 38 | * \endcode 39 | * 40 | */ 41 | 42 | class CartesianControlServerROS2_ParamsParser : public yarp::dev::IDeviceDriverParams 43 | { 44 | public: 45 | CartesianControlServerROS2_ParamsParser(); 46 | ~CartesianControlServerROS2_ParamsParser() override = default; 47 | 48 | public: 49 | const std::string m_device_classname = {"CartesianControlServerROS2"}; 50 | const std::string m_device_name = {"CartesianControlServerROS2"}; 51 | bool m_parser_is_strict = false; 52 | struct parser_version_type 53 | { 54 | int major = 1; 55 | int minor = 0; 56 | }; 57 | const parser_version_type m_parser_version = {}; 58 | 59 | const std::string m_name_defaultValue = {"cartesian_control_server_ros2"}; 60 | const std::string m_fkPeriod_defaultValue = {"20"}; 61 | 62 | std::string m_name = {"cartesian_control_server_ros2"}; 63 | int m_fkPeriod = {20}; 64 | 65 | bool parseParams(const yarp::os::Searchable & config) override; 66 | std::string getDeviceClassName() const override { return m_device_classname; } 67 | std::string getDeviceName() const override { return m_device_name; } 68 | std::string getDocumentationOfDeviceParams() const override; 69 | std::vector getListOfParams() const override; 70 | }; 71 | 72 | #endif 73 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlServerROS2/CartesianControlServerROS2_params.md: -------------------------------------------------------------------------------- 1 | | Group | Parameter | Type | Units | Default Value | Required | Description | Notes | 2 | |:-----:|:------------:|:------:|:-----:|:-----------------------------:|:--------:|:----------------:|:-----:| 3 | | | name | string | | cartesian_control_server_ros2 | no | ROS node name | | 4 | | | fkPeriod | int | ms | 20 | no | FK stream period | | 5 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlServerROS2/DeviceDriverImpl.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include "CartesianControlServerROS2.hpp" 4 | 5 | #include 6 | 7 | #include 8 | 9 | #include "LogComponent.hpp" 10 | 11 | using namespace roboticslab; 12 | 13 | // ------------------- DeviceDriver Related ------------------------------------ 14 | 15 | bool CartesianControlServerROS2::open(yarp::os::Searchable & config) 16 | { 17 | if (!parseParams(config)) 18 | { 19 | yCError(CCS) << "Failed to parse parameters"; 20 | return false; 21 | } 22 | 23 | if (m_fkPeriod > 0) 24 | { 25 | yarp::os::PeriodicThread::setPeriod(m_fkPeriod * 0.001); 26 | } 27 | else 28 | { 29 | yCWarning(CCS) << "Invalid period, using default"; 30 | yarp::os::PeriodicThread::setPeriod(std::stoi(m_fkPeriod_defaultValue) * 0.001); 31 | } 32 | 33 | if (!rclcpp::ok()) 34 | { 35 | rclcpp::init(0, nullptr); 36 | } 37 | 38 | // ROS2 initialization 39 | m_node = std::make_shared(m_name); 40 | 41 | rcl_interfaces::msg::ParameterDescriptor descriptor_streaming_cmd; 42 | descriptor_streaming_cmd.name = "preset_streaming_cmd"; 43 | descriptor_streaming_cmd.description = "Streaming command to be used by the device."; 44 | descriptor_streaming_cmd.read_only = false; 45 | descriptor_streaming_cmd.additional_constraints = "Only 'pose', 'twist', 'wrench' or 'none' are allowed."; 46 | descriptor_streaming_cmd.set__type(rcl_interfaces::msg::ParameterType::PARAMETER_STRING); 47 | 48 | m_node->declare_parameter("preset_streaming_cmd", "none", descriptor_streaming_cmd); 49 | preset_streaming_cmd = VOCAB_CC_NOT_SET; 50 | 51 | rcl_interfaces::msg::ParameterDescriptor descriptor_frame; 52 | descriptor_frame.name = "frame"; 53 | descriptor_frame.description = "Reference frame to be used by the device."; 54 | descriptor_frame.read_only = false; 55 | descriptor_frame.additional_constraints = "Only 'base' or 'tcp' are allowed."; 56 | descriptor_frame.set__type(rcl_interfaces::msg::ParameterType::PARAMETER_STRING); 57 | 58 | m_node->declare_parameter("frame", "base", descriptor_frame); 59 | frame = ICartesianSolver::BASE_FRAME; 60 | 61 | // Spin node for ROS2 62 | m_spinner = new Spinner(m_node); 63 | 64 | if (!m_spinner) 65 | { 66 | yCError(CCS) << "Failed to create spinner"; 67 | return false; 68 | } 69 | 70 | return m_spinner->start(); 71 | } 72 | 73 | // ----------------------------------------------------------------------------- 74 | 75 | bool CartesianControlServerROS2::close() 76 | { 77 | if (m_spinner && m_spinner->isRunning()) 78 | { 79 | return m_spinner->stop(); 80 | } 81 | 82 | return true; 83 | } 84 | 85 | // ----------------------------------------------------------------------------- 86 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlServerROS2/IWrapperImpl.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include "CartesianControlServerROS2.hpp" 4 | 5 | #include 6 | 7 | #include "LogComponent.hpp" 8 | 9 | using namespace roboticslab; 10 | 11 | // ----------------------------------------------------------------------------- 12 | 13 | bool CartesianControlServerROS2::attach(yarp::dev::PolyDriver * poly) 14 | { 15 | if (poly == nullptr) 16 | { 17 | yCError(CCS) << "attach() received nullptr"; 18 | return false; 19 | } 20 | 21 | if (!poly->isValid()) 22 | { 23 | yCError(CCS) << "attach() received invalid PolyDriver"; 24 | return false; 25 | } 26 | 27 | if (!poly->view(m_iCartesianControl)) 28 | { 29 | yCError(CCS) << "attach() failed to obtain ICartesianControl interface"; 30 | return false; 31 | } 32 | 33 | if (!configureRosHandlers()) 34 | { 35 | yCError(CCS) << "Failed to configure ROS handlers"; 36 | destroyRosHandlers(); // cleanup 37 | return false; 38 | } 39 | 40 | return yarp::os::PeriodicThread::start(); 41 | } 42 | 43 | // ----------------------------------------------------------------------------- 44 | 45 | bool CartesianControlServerROS2::detach() 46 | { 47 | yarp::os::PeriodicThread::stop(); 48 | destroyRosHandlers(); 49 | m_iCartesianControl = nullptr; 50 | return true; 51 | } 52 | 53 | // ----------------------------------------------------------------------------- 54 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlServerROS2/LogComponent.cpp: -------------------------------------------------------------------------------- 1 | #include "LogComponent.hpp" 2 | 3 | YARP_LOG_COMPONENT(CCS, "rl.CartesianControlServerROS2") 4 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlServerROS2/LogComponent.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __CARTESIAN_CONTROL_SERVER_ROS2_LOG_COMPONENT_HPP__ 2 | #define __CARTESIAN_CONTROL_SERVER_ROS2_LOG_COMPONENT_HPP__ 3 | 4 | #include 5 | 6 | YARP_DECLARE_LOG_COMPONENT(CCS) 7 | 8 | #endif // __CARTESIAN_CONTROL_SERVER_ROS2_LOG_COMPONENT_HPP__ 9 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlServerROS2/PeriodicThreadImpl.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include "CartesianControlServerROS2.hpp" 4 | 5 | #include // std::modf 6 | 7 | #include 8 | 9 | #include 10 | 11 | #include "LogComponent.hpp" 12 | 13 | using namespace roboticslab; 14 | 15 | // ------------------- PeriodicThread related ------------------------------------ 16 | 17 | void CartesianControlServerROS2::run() 18 | { 19 | std::vector x; 20 | double timestamp; 21 | 22 | if (int state; !m_iCartesianControl->stat(x, &state, ×tamp)) 23 | { 24 | yCWarning(CCS) << "Failed to stat"; 25 | return; 26 | } 27 | 28 | double sec; 29 | double nsec = std::modf(timestamp, &sec) * 1e9; 30 | 31 | const auto rot = KDL::Vector(x[3], x[4], x[5]); 32 | const auto ori = KDL::Rotation::Rot(rot, rot.Norm()); 33 | 34 | geometry_msgs::msg::PoseStamped msg; 35 | msg.header.stamp.sec = sec; 36 | msg.header.stamp.nanosec = nsec; 37 | 38 | msg.pose.position.x = x[0]; 39 | msg.pose.position.y = x[1]; 40 | msg.pose.position.z = x[2]; 41 | 42 | ori.GetQuaternion(msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w); 43 | 44 | m_stat->publish(msg); 45 | } 46 | 47 | // ----------------------------------------------------------------------------- 48 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlServerROS2/Spinner.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include "Spinner.hpp" 4 | 5 | // ------------------- Spinner Class Related ------------------------------------ 6 | 7 | Spinner::Spinner(std::shared_ptr input_node) 8 | : m_node(input_node) 9 | {} 10 | 11 | // ----------------------------------------------------------------------------- 12 | 13 | Spinner::~Spinner() 14 | { 15 | if (m_spun) 16 | { 17 | rclcpp::shutdown(); 18 | m_spun = false; 19 | } 20 | } 21 | 22 | // ----------------------------------------------------------------------------- 23 | 24 | void Spinner::run() 25 | { 26 | if (!m_spun) 27 | { 28 | m_spun = true; 29 | rclcpp::spin(m_node); 30 | } 31 | } 32 | 33 | // ----------------------------------------------------------------------------- 34 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlServerROS2/Spinner.hpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #ifndef __SPINNER_HPP__ 4 | #define __SPINNER_HPP__ 5 | 6 | #include 7 | 8 | #include 9 | 10 | class Spinner : public yarp::os::Thread 11 | { 12 | public: 13 | Spinner(std::shared_ptr input_node); 14 | ~Spinner() override; 15 | void run() override; 16 | 17 | private: 18 | bool m_spun {false}; 19 | std::shared_ptr m_node; 20 | }; 21 | 22 | #endif // __SPINNER_HPP__ 23 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/CartesianControlServerROS2/fig/CartesianControlServer_ROS2-YARP.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/roboticslab-uc3m/kinematics-dynamics/80a0269198a54269b61c5cc3de13abb90a90b8ec/libraries/YarpPlugins/CartesianControlServerROS2/fig/CartesianControlServer_ROS2-YARP.png -------------------------------------------------------------------------------- /libraries/YarpPlugins/KdlSolver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(NOT orocos_kdl_FOUND AND (NOT DEFINED ENABLE_KdlSolver OR ENABLE_KdlSolver)) 2 | message(WARNING "orocos_kdl package not found, disabling KdlSolver") 3 | endif() 4 | 5 | yarp_prepare_plugin(KdlSolver 6 | CATEGORY device 7 | TYPE KdlSolver 8 | INCLUDE KdlSolver.hpp 9 | DEFAULT ON 10 | DEPENDS "ENABLE_ScrewTheoryLib;ENABLE_KdlVectorConverterLib;orocos_kdl_FOUND" 11 | GENERATE_PARSER) 12 | 13 | if(NOT SKIP_KdlSolver) 14 | 15 | yarp_add_plugin(KdlSolver) 16 | 17 | target_sources(KdlSolver PRIVATE KdlSolver.hpp 18 | DeviceDriverImpl.cpp 19 | ICartesianSolverImpl.cpp 20 | ChainFkSolverPos_ST.hpp 21 | ChainFkSolverPos_ST.cpp 22 | ChainIkSolverPos_ST.hpp 23 | ChainIkSolverPos_ST.cpp 24 | ChainIkSolverPos_ID.hpp 25 | ChainIkSolverPos_ID.cpp 26 | KdlSolver_ParamsParser.h 27 | KdlSolver_ParamsParser.cpp 28 | LogComponent.hpp 29 | LogComponent.cpp) 30 | 31 | target_link_libraries(KdlSolver YARP::YARP_os 32 | YARP::YARP_dev 33 | ROBOTICSLAB::ScrewTheoryLib 34 | ROBOTICSLAB::KdlVectorConverterLib 35 | ROBOTICSLAB::KinematicsDynamicsInterfaces 36 | ROBOTICSLAB::KdlSolverUtils) 37 | 38 | yarp_install(TARGETS KdlSolver 39 | LIBRARY DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_DYNAMIC_PLUGINS_INSTALL_DIR} 40 | ARCHIVE DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_STATIC_PLUGINS_INSTALL_DIR} 41 | YARP_INI DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_PLUGIN_MANIFESTS_INSTALL_DIR}) 42 | 43 | else() 44 | 45 | set(ENABLE_KdlSolver OFF CACHE BOOL "Enable/disable KdlSolver device" FORCE) 46 | 47 | endif() 48 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/KdlSolver/ChainFkSolverPos_ST.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include "ChainFkSolverPos_ST.hpp" 4 | 5 | using namespace roboticslab; 6 | 7 | // ----------------------------------------------------------------------------- 8 | 9 | ChainFkSolverPos_ST::ChainFkSolverPos_ST(const KDL::Chain & _chain) 10 | : chain(_chain), 11 | poe(PoeExpression::fromChain(chain)) 12 | {} 13 | 14 | // ----------------------------------------------------------------------------- 15 | 16 | int ChainFkSolverPos_ST::JntToCart(const KDL::JntArray & q_in, KDL::Frame & p_out, int segmentNr) 17 | { 18 | if (segmentNr >= 0) 19 | { 20 | return (error = E_OPERATION_NOT_SUPPORTED); 21 | } 22 | 23 | if (!poe.evaluate(q_in, p_out)) 24 | { 25 | return (error = E_ILLEGAL_ARGUMENT_SIZE); 26 | } 27 | 28 | return (error = E_NOERROR); 29 | } 30 | 31 | // ----------------------------------------------------------------------------- 32 | 33 | int ChainFkSolverPos_ST::JntToCart(const KDL::JntArray & q_in, std::vector & p_out, int segmentNr) 34 | { 35 | return (error = E_OPERATION_NOT_SUPPORTED); 36 | } 37 | 38 | // ----------------------------------------------------------------------------- 39 | 40 | void ChainFkSolverPos_ST::updateInternalDataStructures() 41 | { 42 | poe = PoeExpression::fromChain(chain); 43 | } 44 | 45 | // ----------------------------------------------------------------------------- 46 | 47 | const char * ChainFkSolverPos_ST::strError(const int error) const 48 | { 49 | switch (error) 50 | { 51 | case E_OPERATION_NOT_SUPPORTED: 52 | return "Unsupported operation"; 53 | case E_ILLEGAL_ARGUMENT_SIZE: 54 | return "Illegal argument size"; 55 | default: 56 | return KDL::SolverI::strError(error); 57 | } 58 | } 59 | 60 | // ----------------------------------------------------------------------------- 61 | 62 | KDL::ChainFkSolverPos * ChainFkSolverPos_ST::create(const KDL::Chain & chain) 63 | { 64 | return new ChainFkSolverPos_ST(chain); 65 | } 66 | 67 | // ----------------------------------------------------------------------------- 68 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/KdlSolver/ChainFkSolverPos_ST.hpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #ifndef __CHAIN_FK_SOLVER_POS_ST_HPP__ 4 | #define __CHAIN_FK_SOLVER_POS_ST_HPP__ 5 | 6 | #include 7 | 8 | #include "ProductOfExponentials.hpp" 9 | 10 | namespace roboticslab 11 | { 12 | 13 | /** 14 | * @ingroup KdlSolver 15 | * @brief FK solver using Screw Theory. 16 | * 17 | * Implementation of a forward position kinematics algorithm. This is a thin wrapper 18 | * around \ref PoeExpression. Methods that retrieve resulting frames for intermediate 19 | * links are not supported. 20 | */ 21 | class ChainFkSolverPos_ST : public KDL::ChainFkSolverPos 22 | { 23 | public: 24 | /** 25 | * @brief Perform FK on the selected segment 26 | * 27 | * @param q_in Input joint coordinates. 28 | * @param p_out Reference to output cartesian pose. 29 | * @param segmentNr Desired segment frame (unsupported). 30 | * 31 | * @return Return code, < 0 if something went wrong. 32 | */ 33 | int JntToCart(const KDL::JntArray & q_in, KDL::Frame & p_out, int segmentNr = -1) override; 34 | 35 | /** 36 | * @brief Perform FK on the selected segments (unsupported) 37 | * 38 | * @param q_in Input joint coordinates. 39 | * @param p_out Reference to a vector of output cartesian poses for all segments. 40 | * @param segmentNr Last selected segment frame. 41 | * 42 | * @return Return code, < 0 if something went wrong. 43 | * 44 | * @warning Unsupported, will return @ref E_OPERATION_NOT_SUPPORTED. 45 | */ 46 | int JntToCart(const KDL::JntArray & q_in, std::vector & p_out, int segmentNr = -1) override; 47 | 48 | /** 49 | * @brief Update the internal data structures. 50 | * 51 | * Update the internal data structures. This is required if the number of segments 52 | * or number of joints of a chain has changed. This provides a single point of contact 53 | * for solver memory allocations. 54 | */ 55 | void updateInternalDataStructures() override; 56 | 57 | /** 58 | * @brief Return a description of the last error 59 | * 60 | * @param error Error code. 61 | * 62 | * @return If \p error is known then a description of \p error, otherwise 63 | * "UNKNOWN ERROR". 64 | */ 65 | const char * strError(const int error) const override; 66 | 67 | /** 68 | * @brief Create an instance of \ref ChainFkSolverPos_ST. 69 | * 70 | * @param chain Input kinematic chain. 71 | * 72 | * @return Solver instance. 73 | */ 74 | static KDL::ChainFkSolverPos * create(const KDL::Chain & chain); 75 | 76 | /** @brief Return code, operation not supported. */ 77 | static const int E_OPERATION_NOT_SUPPORTED = -100; 78 | 79 | /** @brief Return code, input vector size does not match expected output vector size. */ 80 | static const int E_ILLEGAL_ARGUMENT_SIZE = -101; 81 | 82 | private: 83 | ChainFkSolverPos_ST(const KDL::Chain & chain); 84 | 85 | const KDL::Chain & chain; 86 | 87 | PoeExpression poe; 88 | }; 89 | 90 | } // namespace roboticslab 91 | 92 | #endif // __CHAIN_FK_SOLVER_POS_ST_HPP__ 93 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/KdlSolver/ChainIkSolverPos_ID.hpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #ifndef __CHAIN_IK_SOLVER_POS_ID_HPP__ 4 | #define __CHAIN_IK_SOLVER_POS_ID_HPP__ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace roboticslab 14 | { 15 | 16 | /** 17 | * @ingroup KdlSolver 18 | * @brief IK solver using infinitesimal displacement twists. 19 | * 20 | * Re-implementation of KDL::ChainIkSolverPos_NR_JL in which only one iteration step 21 | * is performed. Aimed to provide a quick means of obtaining IK whenever the displacements 22 | * are small enough. 23 | */ 24 | class ChainIkSolverPos_ID : public KDL::ChainIkSolverPos 25 | { 26 | public: 27 | /** 28 | * @brief Constructor 29 | * 30 | * @param chain The chain to calculate the inverse position for. 31 | * @param q_min The minimum joint positions. 32 | * @param q_max The maximum joint positions. 33 | * @param fksolver A forward position kinematics solver. 34 | * @param iksolver An inverse velocity kinematics solver. 35 | */ 36 | ChainIkSolverPos_ID(const KDL::Chain & chain, const KDL::JntArray & q_min, const KDL::JntArray & q_max, KDL::ChainFkSolverPos & fksolver); 37 | 38 | /** 39 | * @brief Calculate inverse position kinematics. 40 | * 41 | * @param q_init Initial guess of the joint coordinates (unused). 42 | * @param p_in Input cartesian coordinates. 43 | * @param q_out Output joint coordinates. 44 | * 45 | * @return Return code, \ref E_SOLUTION_NOT_FOUND if there is no solution or 46 | * \ref E_NOT_REACHABLE if at least one of them is out of reach. 47 | */ 48 | int CartToJnt(const KDL::JntArray & q_init, const KDL::Frame & p_in, KDL::JntArray & q_out) override; 49 | 50 | /** 51 | * @brief Update the internal data structures. 52 | * 53 | * Update the internal data structures. This is required if the number of segments 54 | * or number of joints of a chain has changed. This provides a single point of contact 55 | * for solver memory allocations. 56 | */ 57 | void updateInternalDataStructures() override; 58 | 59 | /** 60 | * @brief Return a description of the last error 61 | * 62 | * @param error Error code. 63 | * 64 | * @return If \p error is known then a description of \p error, otherwise 65 | * "UNKNOWN ERROR". 66 | */ 67 | const char * strError(const int error) const override; 68 | 69 | /** @brief Return code, internal FK position solver failed. */ 70 | static const int E_FKSOLVERPOS_FAILED = -100; 71 | 72 | /** @brief Return code, internal Jacobian solver failed. */ 73 | static const int E_JACSOLVER_FAILED = -101; 74 | 75 | private: 76 | KDL::JntArray computeDiffInvKin(const KDL::Twist & delta_twist); 77 | 78 | const KDL::Chain & chain; 79 | unsigned int nj; 80 | 81 | KDL::JntArray qMin; 82 | KDL::JntArray qMax; 83 | 84 | KDL::ChainFkSolverPos & fkSolverPos; 85 | KDL::ChainJntToJacSolver jacSolver; 86 | 87 | KDL::Jacobian jacobian; 88 | }; 89 | 90 | } // namespace roboticslab 91 | 92 | #endif // __CHAIN_IK_SOLVER_POS_ID_HPP__ 93 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/KdlSolver/ChainIkSolverPos_ST.hpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #ifndef __CHAIN_IK_SOLVER_POS_ST_HPP__ 4 | #define __CHAIN_IK_SOLVER_POS_ST_HPP__ 5 | 6 | #include 7 | 8 | #include "ScrewTheoryIkProblem.hpp" 9 | #include "ConfigurationSelector.hpp" 10 | 11 | namespace roboticslab 12 | { 13 | 14 | /** 15 | * @ingroup KdlSolver 16 | * @brief IK solver using Screw Theory. 17 | * 18 | * Implementation of an inverse position kinematics algorithm. This is a thin wrapper 19 | * around \ref ScrewTheoryIkProblem. Non-exhaustive tests on TEO's (UC3M) right arm 20 | * kinematic chain reveal that this is 5-10 faster than a numeric Newton-Raphson 21 | * solver as provided by KDL (e.g. KDL::ChainIkSolverPos_NR_JL). 22 | */ 23 | class ChainIkSolverPos_ST : public KDL::ChainIkSolverPos 24 | { 25 | public: 26 | /** @brief Destructor. */ 27 | virtual ~ChainIkSolverPos_ST(); 28 | 29 | /** 30 | * @brief Calculate inverse position kinematics. 31 | * 32 | * @param q_init Initial guess of the joint coordinates (unused). 33 | * @param p_in Input cartesian coordinates. 34 | * @param q_out Output joint coordinates. 35 | * 36 | * @return Return code, \ref E_SOLUTION_NOT_FOUND if there is no solution or 37 | * \ref E_NOT_REACHABLE if at least one of them is out of reach. 38 | */ 39 | int CartToJnt(const KDL::JntArray & q_init, const KDL::Frame & p_in, KDL::JntArray & q_out) override; 40 | 41 | /** 42 | * @brief Update the internal data structures. 43 | * 44 | * Update the internal data structures. This is required if the number of segments 45 | * or number of joints of a chain has changed. This provides a single point of contact 46 | * for solver memory allocations. 47 | */ 48 | void updateInternalDataStructures() override; 49 | 50 | /** 51 | * @brief Return a description of the last error 52 | * 53 | * @param error Error code. 54 | * 55 | * @return If \p error is known then a description of \p error, otherwise 56 | * "UNKNOWN ERROR". 57 | */ 58 | const char * strError(const int error) const override; 59 | 60 | /** 61 | * @brief Create an instance of \ref ChainIkSolverPos_ST. 62 | * 63 | * @param chain Input kinematic chain. 64 | * @param configFactory Instance of an abstract factory class that 65 | * instantiates a ConfigurationSelector. 66 | * 67 | * @return Solver instance or null if no solution was found. 68 | */ 69 | static KDL::ChainIkSolverPos * create(const KDL::Chain & chain, const ConfigurationSelectorFactory & configFactory); 70 | 71 | /** @brief Return code, IK solution not found. */ 72 | static const int E_SOLUTION_NOT_FOUND = -100; 73 | 74 | /** @brief Return code, target pose out of robot limits. */ 75 | static const int E_OUT_OF_LIMITS = -101; 76 | 77 | /** @brief Return code, solution out of reach. */ 78 | static const int E_NOT_REACHABLE = 100; 79 | 80 | private: 81 | ChainIkSolverPos_ST(const KDL::Chain & chain, ScrewTheoryIkProblem * problem, ConfigurationSelector * config); 82 | 83 | const KDL::Chain & chain; 84 | 85 | ScrewTheoryIkProblem * problem; 86 | 87 | ConfigurationSelector * config; 88 | }; 89 | 90 | } // namespace roboticslab 91 | 92 | #endif // __CHAIN_IK_SOLVER_POS_ST_HPP__ 93 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/KdlSolver/KdlSolver.hpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #ifndef __KDL_SOLVER_HPP__ 4 | #define __KDL_SOLVER_HPP__ 5 | 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "ICartesianSolver.h" 16 | #include "KdlSolver_ParamsParser.h" 17 | #include "LogComponent.hpp" 18 | 19 | /** 20 | * @ingroup YarpPlugins 21 | * @defgroup KdlSolver 22 | * 23 | * @brief Contains KdlSolver. 24 | */ 25 | 26 | /** 27 | * @ingroup KdlSolver 28 | * @brief The KdlSolver class implements ICartesianSolver. 29 | */ 30 | class KdlSolver : public yarp::dev::DeviceDriver, 31 | public roboticslab::ICartesianSolver, 32 | public KdlSolver_ParamsParser 33 | { 34 | public: 35 | // -- ICartesianSolver declarations. Implementation in ICartesianSolverImpl.cpp -- 36 | 37 | // Get number of joints for which the solver has been configured. 38 | int getNumJoints() override; 39 | 40 | // Get number of TCPs for which the solver has been configured. 41 | int getNumTcps() override; 42 | 43 | // Append an additional link. 44 | bool appendLink(const std::vector& x) override; 45 | 46 | // Restore original kinematic chain. 47 | bool restoreOriginalChain() override; 48 | 49 | // Change reference frame. 50 | bool changeOrigin(const std::vector &x_old_obj, const std::vector &x_new_old, std::vector &x_new_obj) override; 51 | 52 | // Perform forward kinematics. 53 | bool fwdKin(const std::vector &q, std::vector &x) override; 54 | 55 | // Obtain difference between supplied pose inputs. 56 | bool poseDiff(const std::vector &xLhs, const std::vector &xRhs, std::vector &xOut) override; 57 | 58 | // Perform inverse kinematics. 59 | bool invKin(const std::vector &xd, const std::vector &qGuess, std::vector &q, reference_frame frame) override; 60 | 61 | // Perform differential inverse kinematics. 62 | bool diffInvKin(const std::vector &q, const std::vector &xdot, std::vector &qdot, reference_frame frame) override; 63 | 64 | // Perform inverse dynamics. 65 | bool invDyn(const std::vector &q, std::vector &t) override; 66 | 67 | // Perform inverse dynamics. 68 | bool invDyn(const std::vector &q, const std::vector &qdot, const std::vector &qdotdot, 69 | const std::vector &ftip, std::vector &t, reference_frame frame) override; 70 | 71 | // -------- DeviceDriver declarations. Implementation in IDeviceImpl.cpp -------- 72 | 73 | bool open(yarp::os::Searchable & config) override; 74 | bool close() override; 75 | 76 | private: 77 | inline const yarp::os::LogComponent & logc() const 78 | { return !m_quiet ? KDLS() : KDLS_QUIET(); } 79 | 80 | mutable std::mutex mtx; 81 | 82 | KDL::Chain chain; 83 | KDL::Chain originalChain; 84 | 85 | KDL::ChainFkSolverPos * fkSolverPos {nullptr}; 86 | KDL::ChainIkSolverPos * ikSolverPos {nullptr}; 87 | KDL::ChainIkSolverVel * ikSolverVel {nullptr}; 88 | KDL::ChainIdSolver * idSolver {nullptr}; 89 | }; 90 | 91 | #endif // __KDL_SOLVER_HPP__ 92 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/KdlSolver/KdlSolver_params.md: -------------------------------------------------------------------------------- 1 | | Group | Parameter | Type | Units | Default Value | Required | Description | Notes | 2 | |:-----:|:--------------:|:--------------:|:-----:|:-------------------------------:|:--------:|:-------------------------------------------------:|:-----------------:| 3 | | | quiet | bool | | false | no | disable logging | | 4 | | | kinematics | string | | | no | path to file with description of robot kinematics | | 5 | | | gravity | vector | m/s^2 | (0.0 0.0 9.81) | no | gravity vector | | 6 | | | ikPos | string | | st | no | IK position solver algorithm | lma, nrjl, st, id | 7 | | | ikVel | string | | pinv | no | IK velocity solver algorithm | pinv, wdls | 8 | | | epsPos | double | m | 1e-5 | no | IK position solver precision | | 9 | | | maxIterPos | int | | 1000 | no | IK position solver max iterations | | 10 | | | epsVel | double | m | 1e-5 | no | IK velocity solver precision | | 11 | | | maxIterVel | int | | 150 | no | IK velocity solver max iterations | | 12 | | | lambda | double | | 0.01 | no | lambda parameter for diff IK | | 13 | | | weightsLMA | vector | | (1.0 1.0 1.0 0.1 0.1 0.1) | no | LMA algorithm weights | | 14 | | | weightsJS | vector | | | no | joint space weights | | 15 | | | weightsTS | vector | | | no | task space weights | | 16 | | | invKinStrategy | string | | leastOverallAngularDisplacement | no | IK configuration strategy | | 17 | | | mins | vector | deg | | yes | lower bound joint position limits | | 18 | | | maxs | vector | deg | | yes | upper bound joint position limits | | 19 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/KdlSolver/LogComponent.cpp: -------------------------------------------------------------------------------- 1 | #include "LogComponent.hpp" 2 | 3 | YARP_LOG_COMPONENT(KDLS, "rl.KdlSolver") 4 | 5 | YARP_LOG_COMPONENT(KDLS_QUIET, "rl.KdlSolver", 6 | yarp::os::Log::minimumPrintLevel(), 7 | yarp::os::Log::minimumForwardLevel(), 8 | nullptr, // disable print 9 | nullptr) // disable forward 10 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/KdlSolver/LogComponent.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __KDL_SOLVER_LOG_COMPONENT_HPP__ 2 | #define __KDL_SOLVER_LOG_COMPONENT_HPP__ 3 | 4 | #include 5 | 6 | YARP_DECLARE_LOG_COMPONENT(KDLS) 7 | YARP_DECLARE_LOG_COMPONENT(KDLS_QUIET) 8 | 9 | #endif // __KDL_SOLVER_LOG_COMPONENT_HPP__ 10 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/KdlTreeSolver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(NOT orocos_kdl_FOUND AND (NOT DEFINED ENABLE_KdlTreeSolver OR ENABLE_KdlTreeSolver)) 2 | message(WARNING "orocos_kdl package not found, disabling KdlTreeSolver") 3 | endif() 4 | 5 | yarp_prepare_plugin(KdlTreeSolver 6 | CATEGORY device 7 | TYPE KdlTreeSolver 8 | INCLUDE KdlTreeSolver.hpp 9 | DEFAULT ON 10 | DEPENDS "ENABLE_KdlVectorConverterLib;orocos_kdl_FOUND" 11 | GENERATE_PARSER) 12 | 13 | if(NOT SKIP_KdlTreeSolver) 14 | 15 | yarp_add_plugin(KdlTreeSolver) 16 | 17 | target_sources(KdlTreeSolver PRIVATE KdlTreeSolver.hpp 18 | DeviceDriverImpl.cpp 19 | ICartesianSolverImpl.cpp 20 | KdlTreeSolver_ParamsParser.h 21 | KdlTreeSolver_ParamsParser.cpp 22 | LogComponent.hpp 23 | LogComponent.cpp) 24 | 25 | target_link_libraries(KdlTreeSolver YARP::YARP_os 26 | YARP::YARP_dev 27 | ROBOTICSLAB::KdlVectorConverterLib 28 | ROBOTICSLAB::KinematicsDynamicsInterfaces 29 | ROBOTICSLAB::KdlSolverUtils) 30 | 31 | yarp_install(TARGETS KdlTreeSolver 32 | LIBRARY DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_DYNAMIC_PLUGINS_INSTALL_DIR} 33 | ARCHIVE DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_STATIC_PLUGINS_INSTALL_DIR} 34 | YARP_INI DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_PLUGIN_MANIFESTS_INSTALL_DIR}) 35 | 36 | else() 37 | 38 | set(ENABLE_KdlTreeSolver OFF CACHE BOOL "Enable/disable KdlTreeSolver device" FORCE) 39 | 40 | endif() 41 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/KdlTreeSolver/KdlTreeSolver.hpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #ifndef __KDL_TREE_SOLVER_HPP__ 4 | #define __KDL_TREE_SOLVER_HPP__ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include "ICartesianSolver.h" 18 | #include "KdlTreeSolver_ParamsParser.h" 19 | 20 | /** 21 | * @ingroup YarpPlugins 22 | * @defgroup KdlTreeSolver 23 | * 24 | * @brief Contains KdlTreeSolver. 25 | */ 26 | 27 | /** 28 | * @ingroup KdlTreeSolver 29 | * @brief The KdlTreeSolver class implements ICartesianSolver. 30 | */ 31 | class KdlTreeSolver : public yarp::dev::DeviceDriver, 32 | public roboticslab::ICartesianSolver, 33 | public KdlTreeSolver_ParamsParser 34 | { 35 | public: 36 | // -- ICartesianSolver declarations. Implementation in ICartesianSolverImpl.cpp -- 37 | 38 | // Get number of joints for which the solver has been configured. 39 | int getNumJoints() override; 40 | 41 | // Get number of TCPs for which the solver has been configured. 42 | int getNumTcps() override; 43 | 44 | // Append an additional link. 45 | bool appendLink(const std::vector & x) override; 46 | 47 | // Restore original kinematic chain. 48 | bool restoreOriginalChain() override; 49 | 50 | // Change reference frame. 51 | bool changeOrigin(const std::vector & x_old_obj, const std::vector & x_new_old, std::vector & x_new_obj) override; 52 | 53 | // Perform forward kinematics. 54 | bool fwdKin(const std::vector & q, std::vector & x) override; 55 | 56 | // Obtain difference between supplied pose inputs. 57 | bool poseDiff(const std::vector & xLhs, const std::vector & xRhs, std::vector & xOut) override; 58 | 59 | // Perform inverse kinematics. 60 | bool invKin(const std::vector & xd, const std::vector & qGuess, std::vector & q, reference_frame frame) override; 61 | 62 | // Perform differential inverse kinematics. 63 | bool diffInvKin(const std::vector & q, const std::vector & xdot, std::vector & qdot, reference_frame frame) override; 64 | 65 | // Perform inverse dynamics. 66 | bool invDyn(const std::vector & q, std::vector & t) override; 67 | 68 | // Perform inverse dynamics. 69 | bool invDyn(const std::vector & q, const std::vector & qdot, const std::vector & qdotdot, 70 | const std::vector & ftip, std::vector & t, reference_frame frame) override; 71 | 72 | // -------- DeviceDriver declarations. Implementation in IDeviceImpl.cpp -------- 73 | 74 | bool open(yarp::os::Searchable & config) override; 75 | 76 | bool close() override; 77 | 78 | protected: 79 | bool makeTree(const yarp::os::Searchable & config); 80 | 81 | std::vector endpoints; 82 | std::map mergedEndpoints; 83 | 84 | KDL::Tree tree; 85 | 86 | KDL::TreeFkSolverPos * fkSolverPos {nullptr}; 87 | KDL::TreeIkSolverPos * ikSolverPos {nullptr}; 88 | KDL::TreeIkSolverVel * ikSolverVel {nullptr}; 89 | KDL::TreeIdSolver * idSolver {nullptr}; 90 | }; 91 | 92 | #endif // __KDL_TREE_SOLVER_HPP__ 93 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/KdlTreeSolver/KdlTreeSolver_params.md: -------------------------------------------------------------------------------- 1 | | Group | Parameter | Type | Units | Default Value | Required | Description | Notes | 2 | |:-----:|:----------:|:--------------:|:-----:|:--------------:|:--------:|:-------------------------------------------------:|:------------:| 3 | | | kinematics | string | | | no | path to file with description of robot kinematics | | 4 | | | gravity | vector | m/s^2 | (0.0 0.0 9.81) | no | gravity vector | | 5 | | | ikPos | string | | nrjl | no | IK position solver algorithm | nrjl, online | 6 | | | epsPos | double | m | 1e-5 | no | IK position solver precision | | 7 | | | maxIterPos | int | | 1000 | no | IK position solver max iterations | | 8 | | | vTranslMax | double | m/s | 1.0 | no | maximum translation speed | | 9 | | | vRotMax | double | deg/s | 50.0 | no | maximum rotation speed | | 10 | | | lambda | double | | 0.01 | no | lambda parameter for diff IK | | 11 | | | weightsJS | vector | | | no | joint space weights | | 12 | | | weightsTS | vector | | | no | task space weights | | 13 | | | mins | vector | deg | | yes | lower bound joint position limits | | 14 | | | maxs | vector | deg | | yes | upper bound joint position limits | | 15 | | | maxvels | vector | deg/s | | yes | joint velocity limits | | 16 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/KdlTreeSolver/LogComponent.cpp: -------------------------------------------------------------------------------- 1 | #include "LogComponent.hpp" 2 | 3 | YARP_LOG_COMPONENT(KDLS, "rl.KdlTreeSolver") 4 | -------------------------------------------------------------------------------- /libraries/YarpPlugins/KdlTreeSolver/LogComponent.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __KDL_TREE_SOLVER_LOG_COMPONENT_HPP__ 2 | #define __KDL_TREE_SOLVER_LOG_COMPONENT_HPP__ 3 | 4 | #include 5 | 6 | YARP_DECLARE_LOG_COMPONENT(KDLS) 7 | 8 | #endif // __KDL_TREE_SOLVER_LOG_COMPONENT_HPP__ 9 | -------------------------------------------------------------------------------- /libraries/YarpTinyMathLib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(NOT YARP_math_FOUND AND (NOT DEFINED ENABLE_YarpTinyMathLib OR ENABLE_YarpTinyMathLib)) 2 | message(WARNING "YARP_math lib not found, disabling YarpTinyMathLib") 3 | endif() 4 | 5 | cmake_dependent_option(ENABLE_YarpTinyMathLib "Enable/disable YarpTinyMathLib library" ON 6 | YARP_math_FOUND OFF) 7 | 8 | if(ENABLE_YarpTinyMathLib) 9 | 10 | add_library(YarpTinyMathLib SHARED YarpTinyMath.hpp 11 | YarpTinyMath.cpp) 12 | 13 | set_target_properties(YarpTinyMathLib PROPERTIES PUBLIC_HEADER YarpTinyMath.hpp) 14 | 15 | target_link_libraries(YarpTinyMathLib PUBLIC YARP::YARP_sig 16 | PRIVATE YARP::YARP_math) 17 | 18 | target_include_directories(YarpTinyMathLib PUBLIC $ 19 | $) 20 | 21 | install(TARGETS YarpTinyMathLib 22 | EXPORT ROBOTICSLAB_KINEMATICS_DYNAMICS) 23 | 24 | set_property(GLOBAL APPEND PROPERTY _exported_dependencies YARP_sig) 25 | 26 | add_library(ROBOTICSLAB::YarpTinyMathLib ALIAS YarpTinyMathLib) 27 | 28 | else() 29 | 30 | set(ENABLE_YarpTinyMathLib OFF CACHE BOOL "Enable/disable YarpTinyMathLib library" FORCE) 31 | 32 | endif() 33 | -------------------------------------------------------------------------------- /libraries/YarpTinyMathLib/YarpTinyMath.hpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #ifndef __YARP_TINY_MATH_HPP__ 4 | #define __YARP_TINY_MATH_HPP__ 5 | 6 | #define _USE_MATH_DEFINES // see on Windows 7 | #include // provides: M_PI 8 | 9 | #include 10 | #include 11 | 12 | namespace roboticslab 13 | { 14 | 15 | /** 16 | * @ingroup kinematics-dynamics-libraries 17 | * @defgroup TinyMath 18 | * @brief A small mathematical library, mostly for performing transformations between angle representations. 19 | */ 20 | 21 | /** 22 | * Simple function to pass from radians to degrees. 23 | * @param inRad angle value in radians. 24 | * @return angle value in degrees. 25 | */ 26 | double toDeg(const double inRad); 27 | 28 | /** 29 | * Simple function to pass from degrees to radians. 30 | * @param inDeg angle value in degrees. 31 | * @return angle value in radians. 32 | */ 33 | double toRad(const double inDeg); 34 | 35 | /** 36 | * Update x values of an homogeneous matrix. 37 | * @param x 3-vector input. 38 | * @param H homogeneous matrix (4x4) that will be modified. 39 | */ 40 | void xUpdateH(const yarp::sig::Vector &x, yarp::sig::Matrix &H); 41 | 42 | yarp::sig::Matrix rotX(const double &inDeg); 43 | yarp::sig::Matrix rotY(const double &inDeg); 44 | yarp::sig::Matrix rotZ(const double &inDeg); 45 | 46 | /** 47 | * @param x 3-vector in meters. 48 | * @param o 3-vector in degrees. 49 | * @return Homogeneous matrix (4x4). 50 | */ 51 | yarp::sig::Matrix eulerZYZtoH(const yarp::sig::Vector &x, const yarp::sig::Vector &o); 52 | 53 | /** 54 | * Uses x to compute rot(Z). 55 | * @param x 3-vector in meters. 56 | * @param o 2-vector in degrees. 57 | * @return Homogeneous matrix (4x4). 58 | */ 59 | yarp::sig::Matrix eulerYZtoH(const yarp::sig::Vector &x, const yarp::sig::Vector &o); 60 | 61 | /** 62 | * Thanks [Ugo Pattacini, Serena Ivaldi, Francesco Nori ((iCub ctrllib/math.h))] for axis2dcm(). 63 | * @param x 3-vector in meters. 64 | * @param o 4-vector in degrees. 65 | * @return Homogeneous matrix (4x4). 66 | */ 67 | yarp::sig::Matrix axisAngleToH(const yarp::sig::Vector &x, const yarp::sig::Vector &o); 68 | 69 | } // namespace roboticslab 70 | 71 | #endif // __YARP_TINY_MATH_HPP__ 72 | 73 | -------------------------------------------------------------------------------- /programs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # programs 2 | 3 | add_subdirectory(ftCompensation) 4 | add_subdirectory(haarDetectionController) 5 | add_subdirectory(keyboardController) 6 | add_subdirectory(streamingDeviceController) 7 | -------------------------------------------------------------------------------- /programs/ftCompensation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(NOT orocos_kdl_FOUND AND (NOT DEFINED ENABLE_ftCompensation OR ENABLE_ftCompensation)) 2 | message(WARNING "orocos_kdl package not found, disabling ftCompensation") 3 | endif() 4 | 5 | cmake_dependent_option(ENABLE_ftCompensation "Enable/disable ftCompensation program" ON 6 | "ENABLE_KdlVectorConverterLib;orocos_kdl_FOUND" OFF) 7 | 8 | if(ENABLE_ftCompensation) 9 | 10 | add_executable(ftCompensation main.cpp 11 | FtCompensation.hpp 12 | FtCompensation.cpp) 13 | 14 | target_link_libraries(ftCompensation ${orocos_kdl_LIBRARIES} 15 | YARP::YARP_os 16 | YARP::YARP_init 17 | YARP::YARP_dev 18 | ROBOTICSLAB::KdlVectorConverterLib 19 | ROBOTICSLAB::KinematicsDynamicsInterfaces) 20 | 21 | target_include_directories(ftCompensation PRIVATE ${orocos_kdl_INCLUDE_DIRS}) 22 | 23 | install(TARGETS ftCompensation) 24 | 25 | else() 26 | 27 | set(ENABLE_ftCompensation OFF CACHE BOOL "Enable/disable ftCompensation program" FORCE) 28 | 29 | endif() 30 | -------------------------------------------------------------------------------- /programs/ftCompensation/FtCompensation.hpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #ifndef __FT_COMPENSATION_HPP__ 4 | #define __FT_COMPENSATION_HPP__ 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include "ICartesianControl.h" 15 | 16 | namespace roboticslab 17 | { 18 | 19 | /** 20 | * @ingroup ftCompensation 21 | * 22 | * @brief Produces motion in the direction of an externally applied force 23 | * measured by a force-torque sensor (pretty much mimicking a classical 24 | * gravity compensation app). 25 | */ 26 | class FtCompensation : public yarp::os::RFModule, 27 | public yarp::os::PeriodicThread 28 | { 29 | public: 30 | FtCompensation() 31 | : yarp::os::PeriodicThread(1.0, yarp::os::ShouldUseSystemClock::Yes, yarp::os::PeriodicThreadClock::Absolute) 32 | {} 33 | 34 | ~FtCompensation() override 35 | { close(); } 36 | 37 | bool configure(yarp::os::ResourceFinder & rf) override; 38 | bool updateModule() override; 39 | bool interruptModule() override; 40 | double getPeriod() override; 41 | bool close() override; 42 | 43 | protected: 44 | void run() override; 45 | 46 | private: 47 | bool readSensor(KDL::Wrench & wrench) const; 48 | bool compensateTool(KDL::Wrench & wrench) const; 49 | bool applyImpedance(KDL::Wrench & wrench); 50 | 51 | yarp::dev::PolyDriver cartesianDevice; 52 | ICartesianControl * iCartesianControl; 53 | 54 | int sensorIndex; 55 | yarp::dev::PolyDriver sensorDevice; 56 | yarp::dev::ISixAxisForceTorqueSensors * sensor; 57 | 58 | KDL::Rotation R_N_sensor; 59 | KDL::Vector toolCoM_N; 60 | KDL::Wrench toolWeight_0; 61 | KDL::Wrench initialOffset; 62 | KDL::Frame initialPose; 63 | KDL::Frame previousPose; 64 | 65 | using cartesian_cmd = void (ICartesianControl::*)(const std::vector &); 66 | cartesian_cmd command; 67 | 68 | bool dryRun; 69 | bool usingTool; 70 | bool enableImpedance; 71 | 72 | double linGain; 73 | double rotGain; 74 | 75 | double linStiffness; 76 | double rotStiffness; 77 | 78 | double linDamping; 79 | double rotDamping; 80 | 81 | double forceDeadband; 82 | double torqueDeadband; 83 | }; 84 | 85 | } // namespace roboticslab 86 | 87 | #endif // __FT_COMPENSATION_HPP__ 88 | -------------------------------------------------------------------------------- /programs/ftCompensation/main.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "FtCompensation.hpp" 8 | 9 | /** 10 | * @ingroup kinematics-dynamics-programs 11 | * 12 | * @defgroup ftCompensation ftCompensation 13 | * 14 | * @brief Creates an instance of roboticslab::FtCompensation. 15 | */ 16 | 17 | int main(int argc, char * argv[]) 18 | { 19 | yarp::os::ResourceFinder rf; 20 | rf.setDefaultContext("ftCompensation"); 21 | rf.setDefaultConfigFile("ftCompensation.ini"); 22 | rf.configure(argc, argv); 23 | 24 | roboticslab::FtCompensation mod; 25 | 26 | yInfo() << "ftCompensation checking for YARP network..."; 27 | 28 | if (!yarp::os::Network::checkNetwork()) 29 | { 30 | yError() << "ftCompensation found no YARP network (try running \"yarpserver &\")"; 31 | return 1; 32 | } 33 | 34 | return mod.runModule(rf); 35 | } 36 | -------------------------------------------------------------------------------- /programs/haarDetectionController/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(NOT TARGET ROBOTICSLAB::YarpDevicesInterfaces AND (NOT DEFINED ENABLE_haarDetectionController OR ENABLE_haarDetectionController)) 2 | message(WARNING "ROBOTICSLAB::YarpDevicesInterfaces target not found, disabling haarDetectionController program") 3 | endif() 4 | 5 | cmake_dependent_option(ENABLE_haarDetectionController "Enable/disable haarDetectionController program" ON 6 | "TARGET ROBOTICSLAB::YarpDevicesInterfaces" OFF) 7 | 8 | if(ENABLE_haarDetectionController) 9 | 10 | add_executable(haarDetectionController main.cpp 11 | HaarDetectionController.hpp 12 | HaarDetectionController.cpp 13 | GrabberResponder.hpp 14 | GrabberResponder.cpp 15 | LogComponent.hpp 16 | LogComponent.cpp) 17 | 18 | target_link_libraries(haarDetectionController YARP::YARP_os 19 | YARP::YARP_init 20 | YARP::YARP_dev 21 | ROBOTICSLAB::KinematicsDynamicsInterfaces 22 | ROBOTICSLAB::YarpDevicesInterfaces) 23 | 24 | install(TARGETS haarDetectionController) 25 | 26 | else() 27 | 28 | set(ENABLE_haarDetectionController OFF CACHE BOOL "Enable/disable haarDetectionController program" FORCE) 29 | 30 | endif() 31 | -------------------------------------------------------------------------------- /programs/haarDetectionController/GrabberResponder.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include "GrabberResponder.hpp" 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include "LogComponent.hpp" 11 | 12 | using namespace roboticslab; 13 | 14 | constexpr double STEP_X = 0.0001; 15 | constexpr double STEP_Y = 0.0001; 16 | constexpr double STEP_Z = 0.01; 17 | 18 | // just in case, not used by now 19 | constexpr double DEADBAND_X_PX = 0.0; 20 | constexpr double DEADBAND_Y_PX = 0.0; 21 | 22 | void GrabberResponder::onRead(yarp::os::Bottle &b) 23 | { 24 | yCDebug(HDC) << "Got:" << b.toString(); 25 | 26 | if (b.size() != 2) 27 | { 28 | yCWarning(HDC) << "Wrong data size:" << b.size() << "(expected: 2)"; 29 | return; 30 | } 31 | 32 | const double input_x = b.get(0).asFloat64(); 33 | const double input_y = b.get(1).asFloat64(); 34 | 35 | double target_x = 0.0; 36 | double target_y = 0.0; 37 | 38 | bool move = false; 39 | 40 | if (std::abs(input_x) > DEADBAND_X_PX) 41 | { 42 | target_x = input_x * STEP_X; 43 | move = true; 44 | } 45 | 46 | if (std::abs(input_y) > DEADBAND_Y_PX) 47 | { 48 | target_y = input_y * STEP_Y; 49 | move = true; 50 | } 51 | 52 | if (move) 53 | { 54 | std::vector xdot(6); 55 | xdot[0] = target_x; 56 | xdot[1] = -target_y; // inverted for AMOR 57 | xdot[2] = noApproach ? 0.0 : STEP_Z; 58 | 59 | iCartesianControl->twist(xdot); 60 | 61 | isStopped = false; 62 | } 63 | else if (!isStopped) 64 | { 65 | iCartesianControl->stopControl(); 66 | isStopped = true; 67 | } 68 | else 69 | { 70 | isStopped = true; 71 | } 72 | } 73 | -------------------------------------------------------------------------------- /programs/haarDetectionController/GrabberResponder.hpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #ifndef __GRABBER_RESPONDER_HPP__ 4 | #define __GRABBER_RESPONDER_HPP__ 5 | 6 | #include 7 | #include 8 | 9 | #include "ICartesianControl.h" 10 | 11 | namespace roboticslab 12 | { 13 | 14 | /** 15 | * @ingroup haarDetectionController 16 | * 17 | * @brief Callback class for dealing with incoming grabber 18 | * data streams. 19 | */ 20 | class GrabberResponder : public yarp::os::TypedReaderCallback 21 | { 22 | public: 23 | GrabberResponder() : iCartesianControl(nullptr), 24 | isStopped(true), 25 | noApproach(false) 26 | {} 27 | 28 | void onRead(yarp::os::Bottle &b) override; 29 | 30 | void setICartesianControlDriver(roboticslab::ICartesianControl *iCartesianControl) 31 | { 32 | this->iCartesianControl = iCartesianControl; 33 | } 34 | 35 | void setNoApproachSetting(bool noApproach) 36 | { 37 | this->noApproach = noApproach; 38 | } 39 | 40 | private: 41 | roboticslab::ICartesianControl *iCartesianControl; 42 | 43 | bool isStopped, noApproach; 44 | }; 45 | 46 | } // namespace roboticslab 47 | 48 | #endif // __GRABBER_RESPONDER_HPP__ 49 | -------------------------------------------------------------------------------- /programs/haarDetectionController/HaarDetectionController.hpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #ifndef __HAAR_DETECTION_CONTROLLER_HPP__ 4 | #define __HAAR_DETECTION_CONTROLLER_HPP__ 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include "ICartesianControl.h" 12 | #include "IProximitySensors.h" 13 | 14 | #include "GrabberResponder.hpp" 15 | 16 | namespace roboticslab 17 | { 18 | 19 | /** 20 | * @ingroup haarDetectionController 21 | * 22 | * @brief Create seek-and-follow trajectories based on 23 | * Haar detection algorithms. 24 | */ 25 | class HaarDetectionController : public yarp::os::RFModule 26 | { 27 | public: 28 | ~HaarDetectionController() override 29 | { close(); } 30 | 31 | bool configure(yarp::os::ResourceFinder & rf) override; 32 | bool updateModule() override; 33 | bool interruptModule() override; 34 | bool close() override; 35 | double getPeriod() override; 36 | 37 | private: 38 | GrabberResponder grabberResponder; 39 | yarp::os::BufferedPort grabberPort; 40 | 41 | yarp::dev::PolyDriver cartesianControlDevice; 42 | roboticslab::ICartesianControl * iCartesianControl; 43 | 44 | yarp::dev::PolyDriver sensorsClientDevice; 45 | roboticslab::IProximitySensors * iProximitySensors; 46 | 47 | double period; 48 | }; 49 | 50 | } // namespace roboticslab 51 | 52 | #endif // __HAAR_DETECTION_CONTROLLER_HPP__ 53 | -------------------------------------------------------------------------------- /programs/haarDetectionController/LogComponent.cpp: -------------------------------------------------------------------------------- 1 | #include "LogComponent.hpp" 2 | 3 | YARP_LOG_COMPONENT(HDC, "rl.HaarDetectionController") 4 | -------------------------------------------------------------------------------- /programs/haarDetectionController/LogComponent.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __HAAR_DETECTION_CONTROLLER_LOG_COMPONENT_HPP__ 2 | #define __HAAR_DETECTION_CONTROLLER_LOG_COMPONENT_HPP__ 3 | 4 | #include 5 | 6 | YARP_DECLARE_LOG_COMPONENT(HDC) 7 | 8 | #endif // __HAAR_DETECTION_CONTROLLER_LOG_COMPONENT_HPP__ 9 | -------------------------------------------------------------------------------- /programs/haarDetectionController/main.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "HaarDetectionController.hpp" 10 | 11 | /** 12 | * @ingroup kinematics-dynamics-programs 13 | * 14 | * @defgroup haarDetectionController haarDetectionController 15 | * 16 | * @brief Creates an instance of roboticslab::HaarDetectionController. 17 | */ 18 | 19 | int main(int argc, char** argv) 20 | { 21 | yarp::os::ResourceFinder rf; 22 | rf.setVerbose(true); 23 | rf.setDefaultContext("haarDetectionController"); 24 | rf.setDefaultConfigFile("haarDetectionController.ini"); 25 | rf.configure(argc, argv); 26 | 27 | roboticslab::HaarDetectionController mod; 28 | 29 | yInfo() << "haarDetectionController checking for yarp network..."; 30 | std::fflush(stdout); 31 | 32 | if (!yarp::os::Network::checkNetwork()) 33 | { 34 | yError() << "haarDetectionController found no yarp network (try running \"yarpserver &\"), bye!"; 35 | return 1; 36 | } 37 | 38 | return mod.runModule(rf); 39 | } 40 | -------------------------------------------------------------------------------- /programs/keyboardController/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(NOT orocos_kdl_FOUND AND (NOT DEFINED ENABLE_keyboardController OR ENABLE_keyboardController)) 2 | message(WARNING "orocos_kdl package not found, disabling keyboardController") 3 | endif() 4 | 5 | cmake_dependent_option(ENABLE_keyboardController "Enable/disable keyboardController program" ON 6 | "ENABLE_KdlVectorConverterLib;ENABLE_KinematicRepresentationLib;orocos_kdl_FOUND" OFF) 7 | 8 | if(ENABLE_keyboardController) 9 | 10 | add_executable(keyboardController main.cpp 11 | KeyboardController.hpp 12 | KeyboardController.cpp 13 | LinearTrajectoryThread.hpp 14 | LinearTrajectoryThread.cpp 15 | LogComponent.hpp 16 | LogComponent.cpp) 17 | 18 | target_link_libraries(keyboardController ${orocos_kdl_LIBRARIES} 19 | YARP::YARP_os 20 | YARP::YARP_init 21 | YARP::YARP_dev 22 | ROBOTICSLAB::KdlVectorConverterLib 23 | ROBOTICSLAB::KinematicRepresentationLib 24 | ROBOTICSLAB::KinematicsDynamicsInterfaces) 25 | 26 | target_include_directories(keyboardController PRIVATE ${orocos_kdl_INCLUDE_DIRS}) 27 | 28 | install(TARGETS keyboardController) 29 | 30 | else() 31 | 32 | set(ENABLE_keyboardController OFF CACHE BOOL "Enable/disable keyboardController program" FORCE) 33 | 34 | endif() 35 | -------------------------------------------------------------------------------- /programs/keyboardController/LinearTrajectoryThread.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include "LinearTrajectoryThread.hpp" 4 | 5 | #include // std::transform 6 | #include // std::bind 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "KdlVectorConverter.hpp" 17 | #include "LogComponent.hpp" 18 | 19 | using namespace roboticslab; 20 | 21 | LinearTrajectoryThread::LinearTrajectoryThread(int _period, ICartesianControl * _iCartesianControl) 22 | : yarp::os::PeriodicThread(_period * 0.001, yarp::os::PeriodicThreadClock::Absolute), 23 | period(_period), 24 | iCartesianControl(_iCartesianControl) 25 | {} 26 | 27 | LinearTrajectoryThread::~LinearTrajectoryThread() 28 | { 29 | delete trajectory; 30 | trajectory = nullptr; 31 | } 32 | 33 | bool LinearTrajectoryThread::checkStreamingConfig() 34 | { 35 | std::map params; 36 | 37 | if (!iCartesianControl->getParameters(params)) 38 | { 39 | yCWarning(KC) << "getParameters failed"; 40 | return false; 41 | } 42 | 43 | usingStreamingCommandConfig = params.find(VOCAB_CC_CONFIG_STREAMING_CMD) != params.end(); 44 | 45 | return true; 46 | } 47 | 48 | bool LinearTrajectoryThread::configure(const std::vector & vels) 49 | { 50 | if (usingStreamingCommandConfig && !iCartesianControl->setParameter(VOCAB_CC_CONFIG_STREAMING_CMD, VOCAB_CC_POSE)) 51 | { 52 | yCWarning(KC) << "Unable to preset streaming command"; 53 | return false; 54 | } 55 | 56 | if (usingTcpFrame) 57 | { 58 | using namespace std::placeholders; 59 | std::vector deltaX(vels.size()); 60 | std::transform(vels.begin(), vels.end(), deltaX.begin(), std::bind(std::multiplies(), period / 1000.0, _1)); 61 | 62 | std::lock_guard lock(mtx); 63 | this->deltaX = deltaX; 64 | return true; 65 | } 66 | 67 | std::vector x; 68 | 69 | if (!iCartesianControl->stat(x)) 70 | { 71 | yCError(KC) << "stat failed"; 72 | return false; 73 | } 74 | 75 | auto H = KdlVectorConverter::vectorToFrame(x); 76 | auto tw = KdlVectorConverter::vectorToTwist(vels); 77 | 78 | std::lock_guard lock(mtx); 79 | 80 | if (trajectory) 81 | { 82 | // discard previous state 83 | delete trajectory; 84 | trajectory = nullptr; 85 | } 86 | 87 | auto * path = new KDL::Path_Line(H, tw, new KDL::RotationalInterpolation_SingleAxis(), 1.0); 88 | auto * profile = new KDL::VelocityProfile_Rectangular(10.0); 89 | profile->SetProfileDuration(0.0, 10.0, 10.0 / path->PathLength()); 90 | trajectory = new KDL::Trajectory_Segment(path, profile); 91 | startTime = yarp::os::Time::now(); 92 | 93 | return true; 94 | } 95 | 96 | void LinearTrajectoryThread::run() 97 | { 98 | if (usingTcpFrame) 99 | { 100 | std::vector deltaX; 101 | 102 | mtx.lock(); 103 | deltaX = this->deltaX; 104 | mtx.unlock(); 105 | 106 | iCartesianControl->pose(deltaX); 107 | } 108 | else 109 | { 110 | mtx.lock(); 111 | auto H = trajectory->Pos(yarp::os::Time::now() - startTime); 112 | mtx.unlock(); 113 | 114 | auto position = KdlVectorConverter::frameToVector(H); 115 | iCartesianControl->pose(position); 116 | } 117 | } 118 | -------------------------------------------------------------------------------- /programs/keyboardController/LinearTrajectoryThread.hpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #ifndef __LINEAR_TRAJECTORY_THREAD_HPP__ 4 | #define __LINEAR_TRAJECTORY_THREAD_HPP__ 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include 12 | 13 | #include "ICartesianControl.h" 14 | 15 | namespace roboticslab 16 | { 17 | 18 | /** 19 | * @ingroup keyboardController 20 | * 21 | * @brief Periodic thread that encapsulates a linear trajectory 22 | */ 23 | class LinearTrajectoryThread : public yarp::os::PeriodicThread 24 | { 25 | public: 26 | LinearTrajectoryThread(int period, ICartesianControl * iCartesianControl); 27 | ~LinearTrajectoryThread() override; 28 | 29 | bool checkStreamingConfig(); 30 | bool configure(const std::vector & vels); 31 | void useTcpFrame(bool enableTcpFrame) { usingTcpFrame = enableTcpFrame; } 32 | 33 | protected: 34 | void run() override; 35 | 36 | private: 37 | double period {0.0}; 38 | ICartesianControl * iCartesianControl {nullptr}; 39 | KDL::Trajectory * trajectory {nullptr}; 40 | double startTime {0.0}; 41 | bool usingStreamingCommandConfig {false}; 42 | bool usingTcpFrame {false}; 43 | std::vector deltaX; 44 | mutable std::mutex mtx; 45 | }; 46 | 47 | } // namespace roboticslab 48 | 49 | #endif // __LINEAR_TRAJECTORY_THREAD_HPP__ 50 | -------------------------------------------------------------------------------- /programs/keyboardController/LogComponent.cpp: -------------------------------------------------------------------------------- 1 | #include "LogComponent.hpp" 2 | 3 | YARP_LOG_COMPONENT(KC, "rl.KeyboardController") 4 | -------------------------------------------------------------------------------- /programs/keyboardController/LogComponent.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __KEYBOARD_CONTROLLER_LOG_COMPONENT_HPP__ 2 | #define __KEYBOARD_CONTROLLER_LOG_COMPONENT_HPP__ 3 | 4 | #include 5 | 6 | YARP_DECLARE_LOG_COMPONENT(KC) 7 | 8 | #endif // __KEYBOARD_CONTROLLER_LOG_COMPONENT_HPP__ 9 | -------------------------------------------------------------------------------- /programs/keyboardController/main.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "KeyboardController.hpp" 10 | 11 | /** 12 | * @ingroup kinematics-dynamics-programs 13 | * 14 | * \defgroup keyboardController keyboardController 15 | * 16 | * @brief Creates an instance of roboticslab::KeyboardController. 17 | */ 18 | 19 | int main(int argc, char *argv[]) 20 | { 21 | yarp::os::ResourceFinder rf; 22 | rf.setVerbose(true); 23 | rf.setDefaultContext("keyboardController"); 24 | rf.setDefaultConfigFile("keyboardController.ini"); 25 | rf.configure(argc, argv); 26 | 27 | roboticslab::KeyboardController mod; 28 | 29 | yInfo() << "keyboardController checking for yarp network..."; 30 | std::fflush(stdout); 31 | 32 | if (!yarp::os::Network::checkNetwork()) 33 | { 34 | yError() << "keyboardController found no yarp network (try running \"yarpserver &\"), bye!"; 35 | return 1; 36 | } 37 | 38 | if (mod.configure(rf)) 39 | { 40 | return mod.runModule(); 41 | } 42 | else 43 | { 44 | return mod.close() ? 0 : 1; 45 | } 46 | } 47 | -------------------------------------------------------------------------------- /programs/streamingDeviceController/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(NOT orocos_kdl_FOUND AND (NOT DEFINED ENABLE_streamingDeviceController OR ENABLE_streamingDeviceController)) 2 | message(WARNING "orocos_kdl package not found, disabling streamingDeviceController") 3 | endif() 4 | 5 | cmake_dependent_option(ENABLE_streamingDeviceController "Enable/disable streamingDeviceController program" ON 6 | "ENABLE_KdlVectorConverterLib;orocos_kdl_FOUND" OFF) 7 | 8 | if(ENABLE_streamingDeviceController) 9 | 10 | # Include any directories needed for this target. 11 | include_directories(${orocos_kdl_INCLUDE_DIRS}) 12 | 13 | # Set up our main executable. 14 | add_executable(streamingDeviceController main.cpp 15 | StreamingDevice.hpp 16 | StreamingDevice.cpp 17 | SpnavSensorDevice.hpp 18 | SpnavSensorDevice.cpp 19 | LeapMotionSensorDevice.hpp 20 | LeapMotionSensorDevice.cpp 21 | WiimoteSensorDevice.hpp 22 | WiimoteSensorDevice.cpp 23 | StreamingDeviceController.hpp 24 | StreamingDeviceController.cpp 25 | CentroidTransform.hpp 26 | CentroidTransform.cpp 27 | LogComponent.hpp 28 | LogComponent.cpp) 29 | 30 | target_link_libraries(streamingDeviceController ${orocos_kdl_LIBRARIES} 31 | YARP::YARP_os 32 | YARP::YARP_init 33 | YARP::YARP_dev 34 | YARP::YARP_sig 35 | ROBOTICSLAB::KdlVectorConverterLib 36 | ROBOTICSLAB::KinematicsDynamicsInterfaces) 37 | 38 | install(TARGETS streamingDeviceController) 39 | 40 | else() 41 | 42 | set(ENABLE_streamingDeviceController OFF CACHE BOOL "Enable/disable streamingDeviceController program" FORCE) 43 | 44 | endif() 45 | -------------------------------------------------------------------------------- /programs/streamingDeviceController/CentroidTransform.cpp: -------------------------------------------------------------------------------- 1 | #include "CentroidTransform.hpp" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | 12 | #include "LogComponent.hpp" 13 | 14 | using namespace roboticslab; 15 | 16 | constexpr auto ROT_FACTOR = 0.1; 17 | 18 | CentroidTransform::CentroidTransform() 19 | : streamingDevice(nullptr), 20 | permanenceTime(0.0) 21 | {} 22 | 23 | bool CentroidTransform::setTcpToCameraRotation(yarp::os::Bottle * b) 24 | { 25 | if (b->size() != 3) 26 | { 27 | yCWarning(SDC) << "Bottle size must equal 3, was:" << b->size(); 28 | return false; 29 | } 30 | 31 | double roll = b->get(0).asFloat64() * M_PI / 180.0; 32 | double pitch = b->get(1).asFloat64() * M_PI / 180.0; 33 | double yaw = b->get(2).asFloat64() * M_PI / 180.0; 34 | 35 | yCInfo(SDC) << "centroidFrameRPY [rad]:" << roll << pitch << yaw; 36 | 37 | rot_tcp_camera = KDL::Rotation::RPY(roll, pitch, yaw); 38 | 39 | return true; 40 | } 41 | 42 | bool CentroidTransform::acceptBottle(yarp::os::Bottle * b) 43 | { 44 | if (b) 45 | { 46 | if (b->size() != 2) 47 | { 48 | yCWarning(SDC) << "Malformed input bottle, size" << b->size() << "(expected 2)"; 49 | return false; 50 | } 51 | 52 | lastBottle = *b; 53 | lastAcquisition.update(); 54 | return true; 55 | } 56 | 57 | return yarp::os::Time::now() - lastAcquisition.getTime() <= permanenceTime; 58 | } 59 | 60 | bool CentroidTransform::processStoredBottle() const 61 | { 62 | // object centroids scaled to fit into [-1, 1] range 63 | double cx = lastBottle.get(0).asFloat64(); // points right 64 | double cy = lastBottle.get(1).asFloat64(); // points down 65 | 66 | std::vector x; 67 | 68 | if (!streamingDevice->iCartesianControl->stat(x)) 69 | { 70 | yCWarning(SDC) << "stat failed"; 71 | return false; 72 | } 73 | 74 | KDL::Frame H_base_tcp = KdlVectorConverter::vectorToFrame(x); 75 | 76 | // express camera's z axis (points "forward") in base frame 77 | KDL::Vector v_base = H_base_tcp.M * rot_tcp_camera * KDL::Vector(0, 0, 1); 78 | KDL::Frame H_base_target = KdlVectorConverter::vectorToFrame(streamingDevice->data); 79 | 80 | double norm = KDL::dot(H_base_target.p, v_base); 81 | 82 | if (norm <= 0.0) 83 | { 84 | // no action if we move away from the target (negative TCP's z axis) 85 | return false; 86 | } 87 | 88 | // project target vector into TCP's z axis, refer result to base frame 89 | H_base_target.p = v_base * norm; 90 | 91 | // find axis along which to rotate (in TCP frame) given pixel coords 92 | KDL::Vector coords(cx, cy, 0); 93 | KDL::Vector tcp_axis = KDL::Rotation::RotZ(KDL::PI_2) * coords; 94 | KDL::Vector base_axis = H_base_tcp.M * rot_tcp_camera * tcp_axis; 95 | 96 | // rotate towards the target in base frame 97 | H_base_target.M = KDL::Rotation::Rot(base_axis, coords.Norm() * ROT_FACTOR); 98 | 99 | // apply changes to input transform 100 | streamingDevice->data = KdlVectorConverter::frameToVector(H_base_target); 101 | 102 | return true; 103 | } 104 | -------------------------------------------------------------------------------- /programs/streamingDeviceController/CentroidTransform.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __CENTROID_TRANSFORM_HPP__ 2 | #define __CENTROID_TRANSFORM_HPP__ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include "StreamingDevice.hpp" 10 | 11 | namespace roboticslab 12 | { 13 | 14 | class StreamingDevice; 15 | 16 | /** 17 | * @ingroup streamingDeviceController 18 | * 19 | * @see @cite eona2020icarsc 20 | */ 21 | class CentroidTransform 22 | { 23 | public: 24 | //! Constructor 25 | CentroidTransform(); 26 | 27 | //! Register handle to device 28 | void registerStreamingDevice(StreamingDevice * streamingDevice) 29 | { this->streamingDevice = streamingDevice; } 30 | 31 | //! Set TCP to camera frame 32 | bool setTcpToCameraRotation(yarp::os::Bottle * b); 33 | 34 | //! Set new permanence time 35 | void setPermanenceTime(double permanenceTime) 36 | { this->permanenceTime = permanenceTime; } 37 | 38 | //! Register or dismiss incoming bottle 39 | bool acceptBottle(yarp::os::Bottle * b); 40 | 41 | //! Process last stored bottle 42 | bool processStoredBottle() const; 43 | 44 | private: 45 | StreamingDevice * streamingDevice; 46 | double permanenceTime; 47 | yarp::os::Bottle lastBottle; 48 | yarp::os::Stamp lastAcquisition; 49 | KDL::Rotation rot_tcp_camera; 50 | }; 51 | 52 | } // namespace roboticslab 53 | 54 | #endif // __CENTROID_TRANSFORM_HPP__ 55 | -------------------------------------------------------------------------------- /programs/streamingDeviceController/LeapMotionSensorDevice.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __LEAP_MOTION_SENSOR_DEVICE_HPP__ 2 | #define __LEAP_MOTION_SENSOR_DEVICE_HPP__ 3 | 4 | #include "StreamingDevice.hpp" 5 | 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | 12 | namespace roboticslab 13 | { 14 | 15 | /** 16 | * @ingroup streamingDeviceController 17 | * 18 | * @brief Represents a LeapMotion device wrapped as an 19 | * analog sensor by YARP. 20 | */ 21 | class LeapMotionSensorDevice : public StreamingDevice 22 | { 23 | public: 24 | //! Constructor 25 | LeapMotionSensorDevice(yarp::os::Searchable & config, bool usingPose); 26 | 27 | bool acquireInterfaces() override; 28 | 29 | bool initialize(bool usingStreamingPreset) override; 30 | 31 | bool acquireData() override; 32 | 33 | bool transformData(double scaling) override; 34 | 35 | int getActuatorState() override; 36 | 37 | void sendMovementCommand(double timestamp) override; 38 | 39 | void stopMotion() override 40 | {} 41 | 42 | private: 43 | yarp::dev::IAnalogSensor * iAnalogSensor; 44 | 45 | bool usingPose; 46 | 47 | std::vector initialTcpOffset; 48 | std::vector initialLeapOffset; 49 | 50 | KDL::Frame frame_base_leap, frame_ee_leap, frame_leap_ee; 51 | 52 | KDL::Frame previousPose; 53 | double previousTimestamp; 54 | 55 | bool hasActuator; 56 | bool grab, pinch; 57 | }; 58 | 59 | } // namespace roboticslab 60 | 61 | #endif // __LEAP_MOTION_SENSOR_DEVICE_HPP__ 62 | -------------------------------------------------------------------------------- /programs/streamingDeviceController/LogComponent.cpp: -------------------------------------------------------------------------------- 1 | #include "LogComponent.hpp" 2 | 3 | YARP_LOG_COMPONENT(SDC, "rl.StreamingDeviceController") 4 | -------------------------------------------------------------------------------- /programs/streamingDeviceController/LogComponent.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __STREAMING_DEVICE_CONTROLLER_LOG_COMPONENT_HPP__ 2 | #define __STREAMING_DEVICE_CONTROLLER_LOG_COMPONENT_HPP__ 3 | 4 | #include 5 | 6 | YARP_DECLARE_LOG_COMPONENT(SDC) 7 | 8 | #endif // __STREAMING_DEVICE_CONTROLLER_LOG_COMPONENT_HPP__ 9 | -------------------------------------------------------------------------------- /programs/streamingDeviceController/SpnavSensorDevice.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __SPNAV_SENSOR_DEVICE_HPP__ 2 | #define __SPNAV_SENSOR_DEVICE_HPP__ 3 | 4 | #include "StreamingDevice.hpp" 5 | 6 | #include 7 | 8 | namespace roboticslab 9 | { 10 | 11 | /** 12 | * @ingroup streamingDeviceController 13 | * 14 | * @brief Represents a spacenav-compatible device, like the SpaceNavigator 15 | * 6-DOF mouse from 3Dconnexion. 16 | */ 17 | class SpnavSensorDevice : public StreamingDevice 18 | { 19 | public: 20 | //! Constructor 21 | SpnavSensorDevice(yarp::os::Searchable & config, bool usingPose, double gain = 0.0); 22 | 23 | bool acquireInterfaces() override; 24 | 25 | bool initialize(bool usingStreamingPreset) override; 26 | 27 | bool acquireData() override; 28 | 29 | bool transformData(double scaling) override; 30 | 31 | int getActuatorState() override; 32 | 33 | bool hasValidMovementData() const override; 34 | 35 | void sendMovementCommand(double timestamp) override; 36 | 37 | void stopMotion() override; 38 | 39 | private: 40 | yarp::dev::IAnalogSensor * iAnalogSensor; 41 | 42 | std::vector currentX; 43 | 44 | bool usingPose; 45 | double gain; 46 | bool buttonClose; 47 | bool buttonOpen; 48 | }; 49 | 50 | } // namespace roboticslab 51 | 52 | #endif // __SPNAV_SENSOR_DEVICE_HPP__ 53 | -------------------------------------------------------------------------------- /programs/streamingDeviceController/StreamingDeviceController.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __STREAMING_DEVICE_CONTROLLER_HPP__ 2 | #define __STREAMING_DEVICE_CONTROLLER_HPP__ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | #include "StreamingDevice.hpp" 14 | #include "CentroidTransform.hpp" 15 | 16 | #include "ICartesianControl.h" 17 | 18 | namespace roboticslab 19 | { 20 | 21 | /** 22 | * @ingroup streamingDeviceController 23 | * 24 | * @brief Sends streaming commands to the cartesian controller from 25 | * a streaming input device like the 3Dconnexion Space Navigator. 26 | * 27 | * @see @cite lukawski2023jjaa 28 | */ 29 | class StreamingDeviceController : public yarp::os::RFModule, 30 | public yarp::os::TypedReaderCallback 31 | { 32 | public: 33 | ~StreamingDeviceController() override 34 | { close(); } 35 | 36 | bool configure(yarp::os::ResourceFinder & rf) override; 37 | bool updateModule() override; 38 | bool interruptModule() override; 39 | bool close() override; 40 | double getPeriod() override; 41 | void onRead(yarp::os::Bottle & bot) override; 42 | 43 | private: 44 | bool update(double timestamp); 45 | 46 | StreamingDevice * streamingDevice; 47 | 48 | yarp::dev::PolyDriver cartesianControlClientDevice; 49 | roboticslab::ICartesianControl * iCartesianControl; 50 | 51 | yarp::os::BufferedPort proximityPort; 52 | int thresholdAlertHigh; 53 | int thresholdAlertLow; 54 | bool disableSensorsLowLevel; 55 | 56 | yarp::os::BufferedPort centroidPort; 57 | CentroidTransform centroidTransform; 58 | 59 | yarp::os::BufferedPort syncPort; 60 | 61 | double period; 62 | double scaling; 63 | bool isStopped; 64 | }; 65 | 66 | } // namespace roboticslab 67 | 68 | #endif // __STREAMING_DEVICE_CONTROLLER_HPP__ 69 | -------------------------------------------------------------------------------- /programs/streamingDeviceController/WiimoteSensorDevice.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __WIIMOTE_SENSOR_DEVICE_HPP__ 2 | #define __WIIMOTE_SENSOR_DEVICE_HPP__ 3 | 4 | #include "StreamingDevice.hpp" 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #define DEFAULT_STEP 0.01 12 | 13 | namespace roboticslab 14 | { 15 | 16 | /** 17 | * @ingroup streamingDeviceController 18 | * 19 | * @brief Represents a Wiimote device wrapped as an 20 | * analog sensor by YARP. 21 | */ 22 | class WiimoteSensorDevice : public StreamingDevice 23 | { 24 | public: 25 | //! Constructor 26 | WiimoteSensorDevice(yarp::os::Searchable & config, bool usingPose); 27 | 28 | bool acquireInterfaces() override; 29 | 30 | bool initialize(bool usingStreamingPreset) override; 31 | 32 | bool acquireData() override; 33 | 34 | bool transformData(double scaling) override; 35 | 36 | bool hasValidMovementData() const override; 37 | 38 | void sendMovementCommand(double timestamp) override; 39 | 40 | void stopMotion() override; 41 | 42 | private: 43 | enum cmd_mode { NONE, FWD, BKWD, ROT }; 44 | 45 | yarp::dev::IAnalogSensor * iAnalogSensor; 46 | 47 | cmd_mode mode; 48 | 49 | std::vector buffer; 50 | 51 | bool usingPose; 52 | double step; 53 | }; 54 | 55 | } // namespace roboticslab 56 | 57 | #endif // __WIIMOTE_SENSOR_DEVICE_HPP__ 58 | -------------------------------------------------------------------------------- /programs/streamingDeviceController/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "StreamingDeviceController.hpp" 6 | 7 | /** 8 | * @ingroup kinematics-dynamics-programs 9 | * 10 | * \defgroup streamingDeviceController streamingDeviceController 11 | * 12 | * @brief Creates an instance of roboticslab::StreamingDeviceController. 13 | */ 14 | 15 | int main(int argc, char *argv[]) 16 | { 17 | yarp::os::ResourceFinder rf; 18 | rf.setDefaultContext("streamingDeviceController"); 19 | rf.setDefaultConfigFile("streamingDeviceController.ini"); 20 | rf.configure(argc, argv); 21 | 22 | roboticslab::StreamingDeviceController mod; 23 | 24 | yInfo() << "streamingDeviceController checking for yarp network..."; 25 | 26 | if (!yarp::os::Network::checkNetwork()) 27 | { 28 | yError() << "streamingDeviceController found no yarp network (try running \"yarpserver &\"), bye!"; 29 | return 1; 30 | } 31 | 32 | if (mod.configure(rf)) 33 | { 34 | return mod.runModule(); 35 | } 36 | else 37 | { 38 | return mod.close() ? 0 : 1; 39 | } 40 | } 41 | -------------------------------------------------------------------------------- /scripts/python/README.md: -------------------------------------------------------------------------------- 1 | # scripts/python 2 | 3 | ## find-reachable-cuboid 4 | 5 | ### Install dependencies 6 | - Requires Python 3.8+ 7 | - Requires [YARP bindings](https://robots.uc3m.es/installation-guides/install-yarp.html#install-python-bindings) 8 | - Requires kinematics-dynamics bindings 9 | - Defaults hard-coded from https://github.com/roboticslab-uc3m/teo-developer-manual 10 | 11 | ### Run 12 | ```bash 13 | python find-reachable-cuboid.py --help 14 | ``` 15 | 16 | ## kdl-from-csv 17 | 18 | ### Install dependencies 19 | - Requires Python [KDL](https://robots.uc3m.es/installation-guides/install-kdl.html) (PyKDL) 20 | - Defaults hard-coded from https://github.com/roboticslab-uc3m/teo-developer-manual 21 | 22 | ### Help 23 | ```bash 24 | python kdl-from-csv.py -h 25 | ``` 26 | 27 | ### Run 28 | ```bash 29 | python kdl-from-csv.py --dhFileName /home/yo/repos/teo-developer-manual/csv/dh-root-rightLeg.csv 30 | ``` 31 | 32 | ## roboview-from-csv 33 | Provides a kinematic chain description for [roboview](https://github.com/arcoslab/roboview), given a `.csv` file. 34 | 35 | ### Install dependencies 36 | - [roboview](https://github.com/arcoslab/roboview) 37 | - At time of writing, best use `vtk6` branch of fork 38 | - Requires [Python YARP](https://robots.uc3m.es/installation-guides/install-yarp.html#install-bindings) and Python [KDL](https://robots.uc3m.es/installation-guides/install-kdl.html) (PyKDL) 39 | - Examples hard-coded from https://github.com/roboticslab-uc3m/teo-developer-manual 40 | 41 | ### Run 42 | ```bash 43 | yarp server & 44 | ./roboview roboview-from-csv.py 45 | ``` 46 | -------------------------------------------------------------------------------- /scripts/python/kdl-from-csv.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # See README.md 4 | 5 | from __future__ import print_function 6 | 7 | from PyKDL import * 8 | from math import pi 9 | import csv 10 | 11 | import os 12 | home = os.environ['HOME'] 13 | 14 | import argparse 15 | parser = argparse.ArgumentParser() 16 | parser.add_argument("--lengthsFileName", default=home+"/repos/teo-developer-manual/csv/lengths.csv") 17 | parser.add_argument("--dhFileName", default=home+"/repos/teo-developer-manual/csv/dh-root-rightArm.csv") 18 | args = parser.parse_args() 19 | 20 | chain = Chain() 21 | 22 | def degToRad(deg): 23 | return deg*pi/180.0 24 | 25 | #-- initPoss 26 | # Small hack: first replace two digits, e.g. else can replace q1 before q13 27 | twoDigitJoints = {'q'+str(i):'0.0' for i in range(10,28)} # dof 28 | oneDigitjoints = {'q'+str(i):'0.0' for i in range(1,10)} # dof 29 | 30 | #-- lengths 31 | lengthsFile = open(args.lengthsFileName, 'r') 32 | reader = csv.reader(lengthsFile, delimiter=',') 33 | next(reader, None) # skip the header 34 | lengths = {row[0]:str(float(row[1])/1000.0) for row in reader} # [mm] to [m] 35 | print('lengths: ',lengths) 36 | 37 | #-- dh params 38 | dhFile = open(args.dhFileName, 'r') 39 | reader = csv.reader(dhFile, delimiter=',') 40 | 41 | #-- populate chain 42 | next(reader, None) # skip the header 43 | for row in reader: 44 | print('row: ',row) 45 | 46 | linkOffset = row[1] 47 | for first, second in twoDigitJoints.items(): 48 | linkOffset = str(linkOffset).replace(first, second) 49 | for first, second in oneDigitjoints.items(): 50 | linkOffset = str(linkOffset).replace(first, second) 51 | linkOffset = eval(linkOffset) 52 | 53 | linkD = row[2] 54 | for first, second in twoDigitJoints.items(): 55 | linkD = str(linkD).replace(first, second) 56 | for first, second in oneDigitjoints.items(): 57 | linkD = str(linkD).replace(first, second) 58 | for first, second in lengths.items(): 59 | linkD = str(linkD).replace(first, second) 60 | linkD = eval(linkD) 61 | 62 | linkA = row[3] 63 | for first, second in lengths.items(): 64 | linkA = str(linkA).replace(first, second) 65 | linkA = eval(linkA) 66 | 67 | linkAlpha = eval(row[4]) 68 | 69 | print('* linkOffset ', linkOffset) 70 | print('* linkD ', linkD) 71 | print('* linkA ', linkA) 72 | print('* linkAlpha ', linkAlpha) 73 | s = Segment(Joint(Joint.RotZ), 74 | Frame().DH(linkA,degToRad(linkAlpha),linkD,degToRad(linkOffset))) 75 | chain.addSegment(s) 76 | 77 | fksolverpos = ChainFkSolverPos_recursive(chain) 78 | q = JntArray(chain.getNrOfJoints()) 79 | x = Frame() 80 | fksolverpos.JntToCart(q,x) 81 | 82 | #print(x) 83 | 84 | def s(inputValue): 85 | tmp = ('%.15f' % inputValue).rstrip('0').rstrip('.') 86 | if tmp == "-0": 87 | return "0" 88 | else: 89 | return tmp 90 | 91 | print("(", s(x.M[0,0]), s(x.M[0,1]), s(x.M[0,2]), s(x.p[0]), " ", 92 | s(x.M[1,0]), s(x.M[1,1]), s(x.M[1,2]), s(x.p[1]), " ", 93 | s(x.M[2,0]), s(x.M[2,1]), s(x.M[2,2]), s(x.p[2]), " ", 94 | "0 0 0 1 )") 95 | -------------------------------------------------------------------------------- /scripts/python/roboview-from-csv.py: -------------------------------------------------------------------------------- 1 | # See README.md 2 | 3 | from PyKDL import * 4 | from math import pi 5 | import csv 6 | 7 | import os 8 | home = os.environ['HOME'] 9 | 10 | lengthsFileName = home+'/repos/teo-developer-manual/csv/lengths.csv' 11 | dhFileName = home+'/repos/teo-developer-manual/csv/dh-rightArm.csv' 12 | 13 | segments = [] # required for roboview 14 | 15 | def degToRad(deg): 16 | return deg*pi/180.0 17 | 18 | #-- initPoss 19 | # Small hack: first replace two digits, e.g. else can replace q1 before q13 20 | twoDigitJoints = {'q'+str(i):'0.0' for i in range(10,28)} # dof 21 | oneDigitjoints = {'q'+str(i):'0.0' for i in range(1,10)} # dof 22 | 23 | #-- lengths 24 | lengthsFile = open(lengthsFileName, 'r') 25 | reader = csv.reader(lengthsFile, delimiter=',') 26 | next(reader, None) # skip the header 27 | lengths = {row[0]:str(float(row[1])/1000.0) for row in reader} # [mm] to [m] 28 | print('lengths: ',lengths) 29 | 30 | #-- dh params 31 | dhFile = open(dhFileName, 'r') 32 | reader = csv.reader(dhFile, delimiter=',') 33 | 34 | #-- populate segments 35 | next(reader, None) # skip the header 36 | for row in reader: 37 | print('row: ',row) 38 | 39 | linkOffset = row[1] 40 | for first, second in twoDigitJoints.items(): 41 | linkOffset = str(linkOffset).replace(first, second) 42 | for first, second in oneDigitjoints.items(): 43 | linkOffset = str(linkOffset).replace(first, second) 44 | linkOffset = eval(linkOffset) 45 | 46 | linkD = row[2] 47 | for first, second in twoDigitJoints.items(): 48 | linkD = str(linkD).replace(first, second) 49 | for first, second in oneDigitjoints.items(): 50 | linkD = str(linkD).replace(first, second) 51 | for first, second in lengths.items(): 52 | linkD = str(linkD).replace(first, second) 53 | linkD = eval(linkD) 54 | 55 | linkA = row[3] 56 | for first, second in lengths.items(): 57 | linkA = str(linkA).replace(first, second) 58 | linkA = eval(linkA) 59 | 60 | linkAlpha = eval(row[4]) 61 | 62 | print('* linkOffset ', linkOffset) 63 | print('* linkD ', linkD) 64 | print('* linkA ', linkA) 65 | print('* linkAlpha ', linkAlpha) 66 | s = Segment(Joint(Joint.RotZ), 67 | Frame().DH(linkA,degToRad(linkAlpha),linkD,degToRad(linkOffset))) 68 | segments.append(s) 69 | 70 | 71 | #-- populate limits (hard-coded for now) 72 | limits_min = [-180.0] * len(segments) 73 | limits_max = [180.0] * len(segments) 74 | 75 | limits_min = [degToRad(v) for v in limits_min] 76 | limits_max = [degToRad(v) for v in limits_max] 77 | -------------------------------------------------------------------------------- /share/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(ENABLE_ftCompensation) 2 | yarp_install(FILES applications/ftCompensation.xml 3 | DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_APPLICATIONS_INSTALL_DIR}/ftCompensation) 4 | 5 | yarp_install(FILES contexts/ftCompensation-lacquey.ini 6 | contexts/ftCompensation-stump.ini 7 | DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_CONTEXTS_INSTALL_DIR}/ftCompensation) 8 | endif() 9 | 10 | if(ENABLE_streamingDeviceController) 11 | yarp_install(FILES contexts/streamingDeviceController.ini 12 | DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_CONTEXTS_INSTALL_DIR}/streamingDeviceController) 13 | endif() 14 | 15 | if(ENABLE_tests AND ENABLE_AsibotSolver) 16 | yarp_install(FILES contexts/testAsibotSolverFromFile.ini 17 | DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_CONTEXTS_INSTALL_DIR}/testAsibotSolverFromFile) 18 | endif() 19 | 20 | if(ENABLE_tests AND ENABLE_KdlSolver) 21 | yarp_install(FILES contexts/testKdlSolverFromFile.ini 22 | DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_CONTEXTS_INSTALL_DIR}/testKdlSolverFromFile) 23 | endif() 24 | 25 | yarp_install(FILES applications/ymanager.ini 26 | DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_APPLICATIONS_INSTALL_DIR}) 27 | -------------------------------------------------------------------------------- /share/applications/ftCompensation.xml: -------------------------------------------------------------------------------- 1 | 2 | ftCompensation app (left arm stump) 3 | 4 | 5 | deviceBundler 6 | --wrapper_device multipleanalogsensorsserver --attached_device Jr3Pci --name /jr3 --period 5 --filter 2 7 | 2.2.2.105 8 | yarpdev 9 | 10 | 11 | 12 | launchCanBus 13 | --from manipulation-leftArm.ini 14 | manipulation 15 | 16 | 17 | 18 | gnome-terminal 19 | -e "yarp rpc /teo/leftArm/rpc:i" 20 | localhost 21 | 22 | 23 | 24 | BasicCartesianControl 25 | --name /teo/leftArm/CartesianControl --kinematics teo-fixedTrunk-leftArm-fetch.ini --local /teo/leftArm/local --remote /teo/leftArm 26 | localhost 27 | yarpdev 28 | 29 | 30 | 31 | ftCompensation 32 | --from ftCompensation-stump.ini --sensorName ch0 --sensorRemote /jr3 --cartesianRemote /teo/leftArm/CartesianControl 33 | localhost 34 | 35 | 36 | -------------------------------------------------------------------------------- /share/applications/ymanager.ini: -------------------------------------------------------------------------------- 1 | # Path to application description files 2 | apppath = "./" 3 | 4 | # Path to module description files 5 | modpath = "./" 6 | 7 | # Path to resource description files 8 | respath = "./" 9 | 10 | load_subfolders = yes 11 | 12 | # Fault tolerance 13 | # parameters: yes|No 14 | watchdog = no 15 | 16 | 17 | # Module failure awarness (if watchdog is enabled) 18 | # parameters: Ignore|terminate|prompt|recover 19 | module_failure = prompt 20 | 21 | 22 | # Connection failure awareness (if watchdog is enabled) 23 | # parameters: Ignore|terminate|prompt 24 | connection_failure = prompt 25 | 26 | 27 | # Automatically stablish all connection 28 | # parameters: Yes|no 29 | auto_connect = no 30 | 31 | # Appearance 32 | # parameters: No|dark|light 33 | color_theme = dark 34 | 35 | # External editor (e.g. gedit, notepad) 36 | external_editor = gedit 37 | -------------------------------------------------------------------------------- /share/contexts/ftCompensation-lacquey.ini: -------------------------------------------------------------------------------- 1 | /// ftCompensation-lacquey.ini 2 | 3 | /// Tested with teo-fixedTrunk-leftArm-fetch.ini. 4 | /// Current HN is: (0 0 -1 -0.0975 -1 0 0 0 0 1 0 0 0 0 0 1) 5 | /// 6 | /// We estimated that the CoM is approx. 8 cm in -X direction regarding H6 frame, 7 | /// hence we substract 2 cm to Z axis in HN for toolCoM. Assuming approximate 8 | /// tool mass of 0.45 kg. Sensor frame is equal to TCP frame. 9 | 10 | // period 0.02 /// command rate [s] 11 | // dryRun false /// process sensor loops, but dont send motion command 12 | // sensorFrameRPY (0 0 0) /// sensor frame RPY rotation regarding TCP frame [deg] 13 | 14 | // linGain 0.01 /// linear gain 15 | // rotGain 0.02 /// rotational gain 16 | 17 | // forceDeadband 1.0 /// force deadband [N] 18 | // torqueDeadband 1.0 /// torque deadband [Nm] 19 | 20 | // local /ftCompensation /// local port prefix 21 | 22 | // sensorName ch0 /// remote FT sensor name to connect to via MAS client 23 | // sensorRemote /jr3 /// remote FT sensor port to connect to via MAS client 24 | 25 | toolCoM (0 0 -0.02) /// tool CoM regarding to TCP frame 26 | toolWeight (0 0 -4.5) /// tool weight vector regarding to inertial frame 27 | 28 | // cartesianRemote /teo/leftArm/CartesianControl /// remote cartesian port to connect to 29 | -------------------------------------------------------------------------------- /share/contexts/ftCompensation-stump.ini: -------------------------------------------------------------------------------- 1 | /// ftCompensation-stump.ini 2 | 3 | /// Tested with a modified version of teo-fixedTrunk-leftArm-fetch.ini. 4 | /// Current HN is: (0 0 -1 -0.0975 -1 0 0 0 0 1 0 0 0 0 0 1) 5 | /// 6 | /// The 9.75 cm value could be further adjusted for the stump, we estimated 7 | /// that 9 cm fits slightly better. 8 | /// 9 | /// Choose one of these options (this file was prepared for 2.): 10 | /// 1. remove HN rotation, adjust translation and sensorFrameRPY 11 | /// HN (1 0 0 -0.09 0 1 0 0 0 0 1 0 0 0 0 1) 12 | /// sensorFrameRPY (90 0 -90) 13 | /// 2. keep HN rotation (sensor rotation equals TCP), adjust translation: 14 | /// HN (0 0 -1 -0.09 -1 0 0 0 0 1 0 0 0 0 0 1) 15 | 16 | /// Assuming negligible mass, hence not using toolCoM nor toolWeight params. 17 | 18 | // period 0.02 /// command rate [s] 19 | // dryRun false /// process sensor loops, but dont send motion command 20 | // sensorFrameRPY (0 0 0) /// sensor frame RPY rotation regarding TCP frame [deg] 21 | 22 | // linGain 0.01 /// linear gain 23 | // rotGain 0.2 /// rotational gain 24 | 25 | // forceDeadband 1.0 /// force deadband [N] 26 | // torqueDeadband 1.0 /// torque deadband [Nm] 27 | 28 | // local /ftCompensation /// local port prefix 29 | 30 | // sensorName ch0 /// remote FT sensor name to connect to via MAS client 31 | // sensorRemote /jr3 /// remote FT sensor port to connect to via MAS client 32 | 33 | // cartesianRemote /teo/leftArm/CartesianControl /// remote cartesian port to connect to 34 | -------------------------------------------------------------------------------- /share/contexts/streamingDeviceController.ini: -------------------------------------------------------------------------------- 1 | /// streamingDeviceController.ini 2 | 3 | // localCartesian /StreamingDeviceCartesianControlClient /// local cartesian client port 4 | // remoteCartesian /CartesianControl /// remote cartesian controller port 5 | 6 | // period 0.02 /// rate in seconds at which the control loop is called 7 | // scaling 10.0 /// scaling factor (final xdot = device output / scaling factor) 8 | 9 | // streamingDevice SpaceNavigator /// device name 10 | 11 | [SpaceNavigator] /// device configuration 12 | device analogsensorclient 13 | local /SpnavDeviceClient 14 | remote /spacenavigator/mouse 15 | fixedAxes none /// axes with restricted movement, available: (x y z rotx roty rotz) 16 | 17 | [LeapMotionSensor] 18 | device analogsensorclient 19 | local /LeapMotionSensorDeviceClient 20 | remote /leap 21 | fixedAxes none 22 | // leapFrameRPY (180 0 -90) /// LeapMotion frame wrt. TCP (ASIBOT) 23 | // leapFrameRPY (180 0 0) /// LeapMotion frame wrt. TCP (AMOR) 24 | 25 | [WiimoteSensor] 26 | device analogsensorclient 27 | local /WiimoteSensorDeviceClient 28 | remote /wiimote 29 | fixedAxes none 30 | -------------------------------------------------------------------------------- /share/contexts/testAsibotSolverFromFile.ini: -------------------------------------------------------------------------------- 1 | A0 0.3 2 | A1 0.4 3 | A2 0.4 4 | A3 0.3 5 | 6 | invKinStrategy leastOverallAngularDisplacement 7 | -------------------------------------------------------------------------------- /share/contexts/testKdlSolverFromFile.ini: -------------------------------------------------------------------------------- 1 | /// originally leftArmKinematics.ini from TEO 2 | 3 | // H0 computed using asibot-main/example/cpp/testTinyMath.cpp 4 | H0 (0 -1 0 0 0 0 1 0.34692 -1 0 0 0.4967 0 0 0 1) /// H0 [H_4x4] link before anything. 5 | 6 | numLinks 6 7 | 8 | link_0 (offset 0.0) (D 0.0 ) (A 0.0 ) (alpha 90.0) (mass 0 ) (cog 0 0 0) (inertia 0 0 0) 9 | link_1 (offset 90.0) (D 0.0 ) (A 0.0 ) (alpha 90.0) (mass 0 ) (cog 0 0 0) (inertia 0 0 0) 10 | link_2 (offset 90.0) (D 0.32901) (A 0.0 ) (alpha 90.0) (mass 1.750625) (cog 0 0 0) (inertia 0 0 0) 11 | link_3 (offset 0.0) (D 0.0 ) (A 0.0 ) (alpha -90.0) (mass 0 ) (cog 0 0 0) (inertia 0 0 0) 12 | link_4 (offset 0.0) (D 0.202 ) (A 0.0 ) (alpha 90.0) (mass 2.396 ) (cog 0 0 0) (inertia 0 0 0) 13 | link_5 (offset 90.0) (D 0.0 ) (A 0.187496) (alpha 90.0) (mass 0.300 ) (cog 0 0 0) (inertia 0 0 0) 14 | 15 | //ik lma 16 | //weights 1 1 1 0.1 0.1 0.1 17 | -------------------------------------------------------------------------------- /tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(NOT GTestSources_FOUND AND (NOT DEFINED ENABLE_tests OR ENABLE_tests)) 2 | message(WARNING "GTestSources package not found, disabling tests") 3 | endif() 4 | 5 | cmake_dependent_option(ENABLE_tests "Enable/disable unit tests" ON 6 | GTestSources_FOUND OFF) 7 | 8 | if(ENABLE_tests) 9 | 10 | add_subdirectory(${GTestSources_SOURCE_DIR} ${CMAKE_BINARY_DIR}/googletest) 11 | 12 | include_directories(${GTestSources_INCLUDE_DIR}) 13 | 14 | # testKinRepresentation 15 | 16 | if(ENABLE_KinematicRepresentationLib) 17 | add_executable(testKinRepresentation testKinRepresentation.cpp) 18 | 19 | target_link_libraries(testKinRepresentation ROBOTICSLAB::KinematicRepresentationLib 20 | gtest_main) 21 | 22 | gtest_discover_tests(testKinRepresentation) 23 | endif() 24 | 25 | # testScrewTheory 26 | 27 | if(ENABLE_ScrewTheoryLib) 28 | add_executable(testScrewTheory testScrewTheory.cpp) 29 | 30 | target_link_libraries(testScrewTheory ROBOTICSLAB::ScrewTheoryLib 31 | gtest_main) 32 | 33 | gtest_discover_tests(testScrewTheory) 34 | endif() 35 | 36 | # testKdlSolver 37 | 38 | if(ENABLE_KdlSolver) 39 | add_executable(testKdlSolver testKdlSolver.cpp) 40 | 41 | target_link_libraries(testKdlSolver YARP::YARP_os 42 | YARP::YARP_dev 43 | ROBOTICSLAB::KinematicsDynamicsInterfaces 44 | gtest_main) 45 | 46 | gtest_discover_tests(testKdlSolver) 47 | endif() 48 | 49 | # testKdlSolverFromFile 50 | 51 | if(ENABLE_KdlSolver) 52 | add_executable(testKdlSolverFromFile testKdlSolverFromFile.cpp) 53 | 54 | target_link_libraries(testKdlSolverFromFile YARP::YARP_os 55 | YARP::YARP_dev 56 | ROBOTICSLAB::KinematicsDynamicsInterfaces 57 | gtest_main) 58 | 59 | gtest_discover_tests(testKdlSolverFromFile) 60 | endif() 61 | 62 | # testAsibotSolverFromFile 63 | 64 | if(ENABLE_AsibotSolver AND ENABLE_KinematicRepresentationLib) 65 | add_executable(testAsibotSolverFromFile testAsibotSolverFromFile.cpp) 66 | 67 | target_link_libraries(testAsibotSolverFromFile YARP::YARP_os 68 | YARP::YARP_dev 69 | ROBOTICSLAB::KinematicRepresentationLib 70 | ROBOTICSLAB::KinematicsDynamicsInterfaces 71 | gtest_main) 72 | 73 | gtest_discover_tests(testAsibotSolverFromFile) 74 | endif() 75 | 76 | # testBasicCartesianControl 77 | 78 | if(ENABLE_BasicCartesianControl) 79 | add_executable(testBasicCartesianControl testBasicCartesianControl.cpp) 80 | 81 | target_link_libraries(testBasicCartesianControl YARP::YARP_os 82 | YARP::YARP_dev 83 | ROBOTICSLAB::KinematicsDynamicsInterfaces 84 | gtest_main) 85 | 86 | gtest_discover_tests(testBasicCartesianControl) 87 | endif() 88 | 89 | else() 90 | 91 | set(ENABLE_tests OFF CACHE BOOL "Enable/disable unit tests" FORCE) 92 | 93 | endif() 94 | -------------------------------------------------------------------------------- /tests/testKdlSolverFromFile.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include "ICartesianSolver.h" 13 | 14 | namespace roboticslab::test 15 | { 16 | 17 | /** 18 | * @ingroup kinematics-dynamics-tests 19 | * @brief Tests @ref KdlSolver ikin and idyn on a simple mechanism. 20 | */ 21 | class KdlSolverTestFromFile : public testing::Test 22 | { 23 | public: 24 | void SetUp() override 25 | { 26 | yarp::os::ResourceFinder rf; 27 | rf.setDefaultContext("testKdlSolverFromFile"); 28 | rf.setDefaultConfigFile("testKdlSolverFromFile.ini"); 29 | 30 | std::string kinematicsFileFullPath = rf.findFileByName("testKdlSolverFromFile.ini"); 31 | 32 | yarp::os::Property solverOptions; 33 | solverOptions.put("device", yarp::os::Value("KdlSolver")); 34 | solverOptions.put("mins", yarp::os::Value::makeList("-70 -15 -10 -100 -90 -100")); 35 | solverOptions.put("maxs", yarp::os::Value::makeList("45 70 75 10 90 10")); 36 | 37 | if (auto & kinematics = solverOptions.addGroup("KINEMATICS"); !kinematics.fromConfigFile(kinematicsFileFullPath)) 38 | { 39 | yError() << "Could not configure from" << kinematicsFileFullPath; 40 | return; 41 | } 42 | 43 | if (!solverDevice.open(solverOptions)) 44 | { 45 | yError() << "Could not open solver device" << solverOptions.find("device").asString(); 46 | return; 47 | } 48 | 49 | if (!solverDevice.view(iCartesianSolver)) 50 | { 51 | yError() << "Could not view ICartesianSolver in" << solverOptions.find("device").asString(); 52 | return; 53 | } 54 | } 55 | 56 | void TearDown() override 57 | { 58 | solverDevice.close(); 59 | } 60 | 61 | protected: 62 | yarp::dev::PolyDriver solverDevice; 63 | roboticslab::ICartesianSolver * iCartesianSolver; 64 | }; 65 | 66 | TEST_F(KdlSolverTestFromFile, KdlSolverFwdKin1) 67 | { 68 | std::vector q(6, 0.0); 69 | std::vector x; 70 | ASSERT_TRUE(iCartesianSolver->fwdKin(q, x)); 71 | ASSERT_EQ(x.size(), 6); //-- axisAngle (scaled) 72 | ASSERT_NEAR(x[0], 0.0, 1e-9); //-- x 73 | ASSERT_NEAR(x[1], 0.34692, 1e-9); //-- y 74 | ASSERT_NEAR(x[2], -0.221806, 1e-9); //-- z 75 | //-- Not checking orientation for now 76 | } 77 | 78 | TEST_F(KdlSolverTestFromFile, KdlSolverFwdKin2) 79 | { 80 | std::vector q {-90.0, 0.0, 0.0, 0.0, 0.0, 0.0}; 81 | std::vector x; 82 | ASSERT_TRUE(iCartesianSolver->fwdKin(q, x)); 83 | ASSERT_EQ(x.size(), 6); //-- axisAngle (scaled) 84 | ASSERT_NEAR(x[0], 0.718506, 1e-9); //-- x 85 | ASSERT_NEAR(x[1], 0.34692, 1e-9); //-- y 86 | ASSERT_NEAR(x[2], 0.4967, 1e-9); //-- z 87 | //-- Not checking orientation for now 88 | } 89 | 90 | } // namespace roboticslab::test 91 | --------------------------------------------------------------------------------