diff --git a/CMakeLists.txt b/CMakeLists.txt index 703e817b..8e19b542 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.16.3) # =================================================================== # PROJECT SETUP # =================================================================== -project(flexiv_rdk VERSION 1.3.0) +project(flexiv_rdk VERSION 1.4.0) # Configure build type if(NOT CMAKE_BUILD_TYPE) @@ -11,26 +11,40 @@ if(NOT CMAKE_BUILD_TYPE) endif() set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Release" "Debug" "RelWithDebInfo") -# Set static library +# Configure Python RDK +option(INSTALL_PYTHON_RDK "Install Python RDK (to user site packages path)" OFF) +if(INSTALL_PYTHON_RDK) + if(NOT RDK_PYTHON_VERSION) + set(RDK_PYTHON_VERSION 3.10 CACHE STRING "Python version to install RDK for" FORCE) + endif() + set_property(CACHE RDK_PYTHON_VERSION PROPERTY STRINGS "3.10" "3.12") + find_package(Python3 ${RDK_PYTHON_VERSION} EXACT COMPONENTS Interpreter REQUIRED) +endif() + +# Set C++ and Python library filename message("OS: ${CMAKE_SYSTEM_NAME}") message("Processor: ${CMAKE_SYSTEM_PROCESSOR}") if(${CMAKE_SYSTEM_NAME} MATCHES "Linux") if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "x86_64") - set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/lib/libflexiv_rdk.x86_64-linux-gnu.a") + set(RDK_CPP_LIB "libflexiv_rdk.x86_64-linux-gnu.a") + set(RDK_PY_LIB "flexivrdk.cpython-${Python3_VERSION_MAJOR}${Python3_VERSION_MINOR}-x86_64-linux-gnu.so") elseif(${CMAKE_SYSTEM_PROCESSOR} MATCHES "aarch64") - set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/lib/libflexiv_rdk.aarch64-linux-gnu.a") + set(RDK_CPP_LIB "libflexiv_rdk.aarch64-linux-gnu.a") + set(RDK_PY_LIB "flexivrdk.cpython-${Python3_VERSION_MAJOR}${Python3_VERSION_MINOR}-aarch64-linux-gnu.so") else() message(FATAL_ERROR "Linux with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.") endif() elseif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "arm64") - set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/lib/libflexiv_rdk.arm64-darwin.a") + set(RDK_CPP_LIB "libflexiv_rdk.arm64-darwin.a") + set(RDK_PY_LIB "flexivrdk.cpython-${Python3_VERSION_MAJOR}${Python3_VERSION_MINOR}-darwin.so") else() message(FATAL_ERROR "Mac with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.") endif() elseif(${CMAKE_SYSTEM_NAME} MATCHES "Windows") if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "AMD64") - set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/lib/flexiv_rdk.win_amd64.lib") + set(RDK_CPP_LIB "flexiv_rdk.win_amd64.lib") + set(RDK_PY_LIB "flexivrdk.cp${Python3_VERSION_MAJOR}${Python3_VERSION_MINOR}-win_amd64.pyd") else() message(FATAL_ERROR "Windows with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.") endif() @@ -52,6 +66,12 @@ if(Eigen3_FOUND) message(STATUS "Found Eigen3: ${Eigen3_DIR}") endif() +# spdlog +find_package(spdlog REQUIRED) +if(spdlog_FOUND) + message(STATUS "Found spdlog: ${spdlog_DIR}") +endif() + # Fast-CDR find_package(fastcdr 1.0.24 REQUIRED) if(fastcdr_FOUND) @@ -65,7 +85,7 @@ if(fastrtps_FOUND) endif() # =================================================================== -# CREATE LIBRARY +# INSTALL C++ LIBRARY # =================================================================== # Create an INTERFACE library with no source file to compile add_library(${PROJECT_NAME} INTERFACE) @@ -80,9 +100,10 @@ target_include_directories(${PROJECT_NAME} INTERFACE ) target_link_libraries(${PROJECT_NAME} INTERFACE - ${RDK_STATIC_LIBRARY} + ${CMAKE_CURRENT_SOURCE_DIR}/lib/${RDK_CPP_LIB} Threads::Threads Eigen3::Eigen + spdlog::spdlog fastrtps fastcdr ) @@ -95,5 +116,24 @@ else() endif() # Install the INTERFACE library +message(STATUS "C++ RDK library: ${RDK_CPP_LIB}") include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/FlexivInstallLibrary.cmake) FlexivInstallLibrary() + +# =================================================================== +# INSTALL PYTHON LIBRARY +# =================================================================== +if(INSTALL_PYTHON_RDK) + # Get user site packages path + message(STATUS "Python executable: ${Python3_EXECUTABLE}") + execute_process( + COMMAND ${Python3_EXECUTABLE} -c "import site; print(site.getusersitepackages())" + OUTPUT_VARIABLE USER_SITE + OUTPUT_STRIP_TRAILING_WHITESPACE + ) + message(STATUS "Python site packages path: ${USER_SITE}") + + # Install Python RDK to user site packages path + message(STATUS "Python RDK library: ${RDK_PY_LIB}") + install(FILES "${CMAKE_CURRENT_SOURCE_DIR}/lib_py/${RDK_PY_LIB}" DESTINATION ${USER_SITE}) +endif() diff --git a/README.md b/README.md index 9d6ae15f..8f1dddb5 100644 --- a/README.md +++ b/README.md @@ -15,93 +15,108 @@ Flexiv RDK (Robotic Development Kit), a key component of the Flexiv Robotic Soft | -------------------------- | ----------------------- | ---------------------- | ------------------------- | | Linux (Ubuntu 20.04/22.04) | x86_64, arm64 | C++, Python | build-essential | | macOS 12 and above | arm64 | C++, Python | Xcode Command Line Tools | -| Windows 10/11 | x86_64 | C++, Python | MSVC v142+ | +| Windows 10/11 | x86_64 | C++, Python | MSVC v14.2+ | ## Quick Start +The **C++ and Python** RDK libraries are packed into a unified modern CMake project named ``flexiv_rdk``, which can be configured and installed via CMake on all supported OS. + ### Note -* The full documentation is in [Flexiv RDK Manual](https://rdk.flexiv.com/manual/). -* You might need to turn off your computer's firewall to be able to connect to the robot. +* The instructions below serve as a quick reference. You can find the full documentation at [Flexiv RDK Manual](https://rdk.flexiv.com/manual/). +* You might need to turn off your computer's firewall or whitelist the RDK programs to be able to establish connection with the robot. -### C++ RDK +### Install on Linux -The C++ interface of Flexiv RDK is packed into a unified modern CMake library named ``flexiv_rdk``, which can be configured via CMake on all supported OS. +1. In a new Terminal, install compiler kit, CMake (with GUI), Python interpreter, and Python package manager: -NOTE: if you will only be using Python RDK, you can skip this section and jump to [Python RDK](#python-rdk). + sudo apt install build-essential cmake cmake-qt-gui python3 python3-pip -y -#### Compile and install for Linux +2. Choose a directory for installing C++ RDK library and all its dependencies. This directory can be under system path or not, depending on whether you want RDK to be globally discoverable by CMake. For example, a new folder named ``rdk_install`` under the home directory. +3. In a new Terminal, run the provided script to compile and install all C++ dependencies to the installation directory chosen in step 2: -1. In a new Terminal, install C++ compiler, Git, and CMake (with GUI) using the package manager: + cd flexiv_rdk/thirdparty + bash build_and_install_dependencies.sh ~/rdk_install - sudo apt install build-essential git cmake cmake-qt-gui -y +4. In a new Terminal, use CMake to configure the ``flexiv_rdk`` project: -2. Choose a directory for installing ``flexiv_rdk`` library and all its dependencies. For example, a new folder named ``rdk_install`` under the home directory. -3. In a new Terminal, run the provided script to compile and install all dependencies to the installation directory chosen in step 2: + cd flexiv_rdk + mkdir build && cd build + cmake .. -DCMAKE_INSTALL_PREFIX=~/rdk_install -DINSTALL_PYTHON_RDK=ON - cd flexiv_rdk/thirdparty - bash build_and_install_dependencies.sh ~/rdk_install + NOTE: ``-D`` followed by ``CMAKE_INSTALL_PREFIX`` sets the CMake variable that specifies the path of the installation directory. ``-D`` followed by ``INSTALL_PYTHON_RDK=ON`` enables the installation of Python RDK besides C++ RDK. The configuration process can also be done using CMake GUI. -4. In a new Terminal, use CMake to configure ``flexiv_rdk``: +5. Install C++ and Python RDK: - cd flexiv_rdk - mkdir build && cd build - cmake .. -DCMAKE_INSTALL_PREFIX=~/rdk_install + cd flexiv_rdk/build + cmake --build . --target install --config Release - NOTE: ``-D`` followed by ``CMAKE_INSTALL_PREFIX`` is a CMake parameter specifying the path of the chosen installation directory. Alternatively, this configuration step can also be done through CMake GUI. + C++ RDK is installed to the path specified by ``CMAKE_INSTALL_PREFIX``, which may or may not be globally discoverable by CMake. Python RDK is installed to the user site packages path, which is globally discoverable by Python interpreter. -5. Compile and install ``flexiv_rdk`` library: +### Install on macOS - cd flexiv_rdk/build - cmake --build . --target install --config Release +1. Install compiler kit: In a new Terminal, enter command ``xcode-select`` to invoke the installation of Xcode Command Line Tools, then follow the prompted window to finish the installation. +2. Install CMake (with GUI): Download ``cmake-3.x.x-macos-universal.dmg`` from [CMake download page](https://cmake.org/download/) and install the dmg file. The minimum required version is 3.16.3. When done, start CMake from Launchpad and navigate to Tools -> How to Install For Command Line Use. Then follow the instruction "Or, to install symlinks to '/usr/local/bin', run:" to install ``cmake`` and ``cmake-gui`` commands for use in Terminal. +3. Install Python interpreter and package manager (replace "3.x" with the actual Python3 version you wish to use): - NOTE: the installation of ``flexiv_rdk`` library is complete now. The following steps show how to link to the installed library from a user project. + brew install python@3.x -6. To find and link to the installed ``flexiv_rdk`` library from a user project, using the provided example project for instance: +4. The rest are identical to steps 2 and below in [Install on Linux](#install-on-linux). - cd flexiv_rdk/example - mkdir build && cd build - cmake .. -DCMAKE_INSTALL_PREFIX=~/rdk_install - cmake --build . --config Release -j 4 +### Install on Windows - NOTE: ``-D`` followed by ``CMAKE_INSTALL_PREFIX`` tells user project's CMake where to find the installed ``flexiv_rdk`` library. +1. Install compiler kit: Download and install Microsoft Visual Studio 2019 (MSVC v14.2) or above. Choose "Desktop development with C++" under the *Workloads* tab during installation. You only need to keep the following components for the selected workload: + * MSVC ... C++ x64/x86 build tools (Latest) + * C++ CMake tools for Windows + * Windows 10 SDK or Windows 11 SDK, depending on your actual Windows version +2. Install CMake (with GUI): Download ``cmake-3.x.x-windows-x86_64.msi`` from [CMake download page](https://cmake.org/download/) and install the msi file. The minimum required version is 3.16.3. **Add CMake to system PATH** when prompted, so that ``cmake`` and ``cmake-gui`` command can be used from Command Prompt or a bash emulator. +3. Install bash emulator: Download and install [Git for Windows](https://git-scm.com/download/win/), which comes with a bash emulator Git Bash. +4. Within the bash emulator, the rest are identical to steps 2 and below in [Install on Linux](#install-on-linux). -7. Assuming the system setup detailed in the Flexiv RDK Manual is done, to run an compiled example program: +### Link to C++ RDK from a user program - ./ [robot_serial_number] +After C++ RDK is installed, it can be found as a CMake library and linked to by other CMake projects. Use the provided examples project for instance:: - For example: + cd flexiv_rdk/example + mkdir build && cd build + cmake .. -DCMAKE_INSTALL_PREFIX=~/rdk_install + cmake --build . --config Release -j 4 - ./display_robot_states Rizon4s-123456 +NOTE: ``-D`` followed by ``CMAKE_INSTALL_PREFIX`` tells the user project's CMake where to find the installed C++ RDK library. The instruction above applies to all supported OS. - Note: ``sudo`` is not required unless prompted by the program. +### Run example programs -#### Compile and install for macOS +#### C++ -1. In a Terminal, use ``xcode-select`` command to invoke the installation of Xcode Command Line Tools, then follow the prompted window to finish the installation. -2. Download ``cmake-3.x.x-macos-universal.dmg`` from [CMake download page](https://cmake.org/download/) and install the dmg file. The minimum required version is 3.16.3. -3. When done, start CMake from Launchpad and navigate to Tools -> How to Install For Command Line Use. Then follow the instruction "Or, to install symlinks to '/usr/local/bin', run:" to install ``cmake`` and ``cmake-gui`` command for use in Terminal. -4. The rest steps are the same as [Compile and install for Linux](#compile-and-install-for-linux), beginning from step 2. +To run a compiled example C++ program: -#### Compile and install for Windows + cd flexiv_rdk/example/build + ./ [robot_serial_number] -1. Install Microsoft Visual Studio 2019 (MSVC v142) or above . Choose "Desktop development with C++" under the *Workloads* tab during installation. You only need to keep the following components for the selected workload: - * MSVC ... C++ x64/x86 build tools (Latest) - * C++ CMake tools for Windows - * Windows 10 SDK or Windows 11 SDK, depending on your actual Windows version -2. Download ``cmake-3.x.x-windows-x86_64.msi`` from [CMake download page](https://cmake.org/download/) and install the msi file. The minimum required version is 3.16.3. **Add CMake to system PATH** when prompted, so that ``cmake`` and ``cmake-gui`` command can be used from Command Prompt or a bash emulator. -3. Install a bash emulator. Git Bash that comes with Git (for Windows) installation is recommended. -4. Within the bash emulator, the rest steps are the same as [Compile and install for Linux](#compile-and-install-for-linux), beginning from step 2. +For example: + + ./basics1_display_robot_states Rizon4s-123456 + +NOTE: ``sudo`` is only required if the real-time scheduler API ``flexiv::rdk::Scheduler`` is used. + +#### Python + +To run a example Python program: + + cd flexiv_rdk/example_py + python3 .py [robot_serial_number] -### Python RDK +For example: -Python 3.10 and 3.12 are supported by RDK, see Flexiv RDK Manual for more details. A brief instruction is provided below and applies to all supported OS. + python3 ./basics1_display_robot_states.py Rizon4s-123456 -1. Check Python version is 3.10.x or 3.12.x: +## API Documentation - python3 --version +The complete and detailed API documentation of the **latest release** can be found at https://rdk.flexiv.com/api/. The API documentation of a previous release can be generated manually using Doxygen. For example, on Linux: -2. Assume the system setup detailed in Flexiv RDK Manual is done, to run an example Python program: + sudo apt install doxygen-latex graphviz + cd flexiv_rdk + git checkout + doxygen doc/Doxyfile.in - cd flexiv_rdk/example_py - python3 .py [robot_serial_number] +The generated API documentation is under ``flexiv_rdk/doc/html/`` directory. Open any html file with your browser to view it. diff --git a/cmake/flexiv_rdk-config.cmake.in b/cmake/flexiv_rdk-config.cmake.in index eb4d80ba..4b30b336 100644 --- a/cmake/flexiv_rdk-config.cmake.in +++ b/cmake/flexiv_rdk-config.cmake.in @@ -4,6 +4,7 @@ include(CMakeFindDependencyMacro) set(THREADS_PREFER_PTHREAD_FLAG ON) find_dependency(Threads REQUIRED) find_dependency(Eigen3 REQUIRED) +find_dependency(spdlog REQUIRED) find_dependency(fastrtps 2.6.2 REQUIRED) find_dependency(fastcdr 1.0.24 REQUIRED) diff --git a/doc/Doxyfile.in b/doc/Doxyfile.in old mode 100755 new mode 100644 index 13f615dd..f21b05de --- a/doc/Doxyfile.in +++ b/doc/Doxyfile.in @@ -1,4 +1,4 @@ -# Doxyfile 1.8.13 +# Doxyfile 1.9.1 # This file describes the settings to be used by the documentation system # doxygen (www.doxygen.org) for a project. @@ -17,11 +17,11 @@ # Project related configuration options #--------------------------------------------------------------------------- -# This tag specifies the encoding used for all characters in the config file -# that follow. The default is UTF-8 which is also the encoding used for all text -# before the first occurrence of this tag. Doxygen uses libiconv (or the iconv -# built into libc) for the transcoding. See http://www.gnu.org/software/libiconv -# for the list of possible encodings. +# This tag specifies the encoding used for all characters in the configuration +# file that follow. The default is UTF-8 which is also the encoding used for all +# text before the first occurrence of this tag. Doxygen uses libiconv (or the +# iconv built into libc) for the transcoding. See +# https://www.gnu.org/software/libiconv/ for the list of possible encodings. # The default value is: UTF-8. DOXYFILE_ENCODING = UTF-8 @@ -38,13 +38,13 @@ PROJECT_NAME = "Flexiv RDK APIs" # could be handy for archiving the generated documentation or if some version # control system is used. -PROJECT_NUMBER = "1.3" +PROJECT_NUMBER = 1.4 # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a # quick idea about the purpose of the project. Keep the description short. -PROJECT_BRIEF = "" +PROJECT_BRIEF = # With the PROJECT_LOGO tag one can specify a logo or an icon that is included # in the documentation. The maximum height of the logo should not exceed 55 @@ -93,6 +93,14 @@ ALLOW_UNICODE_NAMES = NO OUTPUT_LANGUAGE = English +# The OUTPUT_TEXT_DIRECTION tag is used to specify the direction in which all +# documentation generated by doxygen is written. Doxygen will use this +# information to generate all generated output in the proper direction. +# Possible values are: None, LTR, RTL and Context. +# The default value is: None. + +OUTPUT_TEXT_DIRECTION = None + # If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member # descriptions after the members that are listed in the file and class # documentation (similar to Javadoc). Set to NO to disable this. @@ -189,6 +197,16 @@ SHORT_NAMES = NO JAVADOC_AUTOBRIEF = NO +# If the JAVADOC_BANNER tag is set to YES then doxygen will interpret a line +# such as +# /*************** +# as being the beginning of a Javadoc-style comment "banner". If set to NO, the +# Javadoc-style will behave just like regular comments and it will not be +# interpreted by doxygen. +# The default value is: NO. + +JAVADOC_BANNER = NO + # If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first # line (until the first dot) of a Qt-style comment as the brief description. If # set to NO, the Qt-style will behave just like regular Qt-style comments (thus @@ -209,6 +227,14 @@ QT_AUTOBRIEF = NO MULTILINE_CPP_IS_BRIEF = NO +# By default Python docstrings are displayed as preformatted text and doxygen's +# special commands cannot be used. By setting PYTHON_DOCSTRING to NO the +# doxygen's special commands can be used and the contents of the docstring +# documentation blocks is shown as doxygen documentation. +# The default value is: YES. + +PYTHON_DOCSTRING = YES + # If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the # documentation from any documented member that it re-implements. # The default value is: YES. @@ -236,16 +262,15 @@ TAB_SIZE = 4 # will allow you to put the command \sideeffect (or @sideeffect) in the # documentation, which will result in a user-defined paragraph with heading # "Side Effects:". You can put \n's in the value part of an alias to insert -# newlines. +# newlines (in the resulting output). You can put ^^ in the value part of an +# alias to insert a newline as if a physical newline was in the original file. +# When you need a literal { or } or , in the value part of an alias you have to +# escape them by means of a backslash (\), this can lead to conflicts with the +# commands \{ and \} for these it is advised to use the version @{ and @} or use +# a double escape (\\{ and \\}) ALIASES = -# This tag can be used to specify a number of word-keyword mappings (TCL only). -# A mapping has the form "name=value". For example adding "class=itcl::class" -# will allow you to use the command class in the itcl::class meaning. - -TCL_SUBST = - # Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources # only. Doxygen will then generate output that is more tailored for C. For # instance, some of the names that are used will be different. The list of all @@ -274,28 +299,40 @@ OPTIMIZE_FOR_FORTRAN = NO OPTIMIZE_OUTPUT_VHDL = NO +# Set the OPTIMIZE_OUTPUT_SLICE tag to YES if your project consists of Slice +# sources only. Doxygen will then generate output that is more tailored for that +# language. For instance, namespaces will be presented as modules, types will be +# separated into more groups, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_SLICE = NO + # Doxygen selects the parser to use depending on the extension of the files it # parses. With this tag you can assign which parser to use for a given # extension. Doxygen has a built-in mapping, but you can override or extend it # using this tag. The format is ext=language, where ext is a file extension, and -# language is one of the parsers supported by doxygen: IDL, Java, Javascript, -# C#, C, C++, D, PHP, Objective-C, Python, Fortran (fixed format Fortran: -# FortranFixed, free formatted Fortran: FortranFree, unknown formatted Fortran: -# Fortran. In the later case the parser tries to guess whether the code is fixed -# or free formatted code, this is the default for Fortran type files), VHDL. For -# instance to make doxygen treat .inc files as Fortran files (default is PHP), -# and .f files as C (default is Fortran), use: inc=Fortran f=C. +# language is one of the parsers supported by doxygen: IDL, Java, JavaScript, +# Csharp (C#), C, C++, D, PHP, md (Markdown), Objective-C, Python, Slice, VHDL, +# Fortran (fixed format Fortran: FortranFixed, free formatted Fortran: +# FortranFree, unknown formatted Fortran: Fortran. In the later case the parser +# tries to guess whether the code is fixed or free formatted code, this is the +# default for Fortran type files). For instance to make doxygen treat .inc files +# as Fortran files (default is PHP), and .f files as C (default is Fortran), +# use: inc=Fortran f=C. # # Note: For files without extension you can use no_extension as a placeholder. # # Note that for custom extensions you also need to set FILE_PATTERNS otherwise -# the files are not read by doxygen. +# the files are not read by doxygen. When specifying no_extension you should add +# * to the FILE_PATTERNS. +# +# Note see also the list of default file extension mappings. EXTENSION_MAPPING = # If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments # according to the Markdown format, which allows for more readable -# documentation. See http://daringfireball.net/projects/markdown/ for details. +# documentation. See https://daringfireball.net/projects/markdown/ for details. # The output of markdown processing is further processed by doxygen, so you can # mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in # case of backward compatibilities issues. @@ -307,7 +344,7 @@ MARKDOWN_SUPPORT = YES # to that level are automatically included in the table of contents, even if # they do not have an id attribute. # Note: This feature currently applies only to Markdown headings. -# Minimum value: 0, maximum value: 99, default value: 0. +# Minimum value: 0, maximum value: 99, default value: 5. # This tag requires that the tag MARKDOWN_SUPPORT is set to YES. TOC_INCLUDE_HEADINGS = 0 @@ -337,7 +374,7 @@ BUILTIN_STL_SUPPORT = NO CPP_CLI_SUPPORT = NO # Set the SIP_SUPPORT tag to YES if your project consists of sip (see: -# http://www.riverbankcomputing.co.uk/software/sip/intro) sources only. Doxygen +# https://www.riverbankcomputing.com/software/sip/intro) sources only. Doxygen # will parse them like normal C++ but will assume all classes use public instead # of private inheritance when no explicit protection keyword is present. # The default value is: NO. @@ -423,6 +460,19 @@ TYPEDEF_HIDES_STRUCT = NO LOOKUP_CACHE_SIZE = 0 +# The NUM_PROC_THREADS specifies the number threads doxygen is allowed to use +# during processing. When set to 0 doxygen will based this on the number of +# cores available in the system. You can set it explicitly to a value larger +# than 0 to get more control over the balance between CPU load and processing +# speed. At this moment only the input processing can be done using multiple +# threads. Since this is still an experimental feature the default is set to 1, +# which efficively disables parallel processing. Please report any issues you +# encounter. Generating dot graphs in parallel is controlled by the +# DOT_NUM_THREADS setting. +# Minimum value: 0, maximum value: 32, default value: 1. + +NUM_PROC_THREADS = 1 + #--------------------------------------------------------------------------- # Build related configuration options #--------------------------------------------------------------------------- @@ -443,6 +493,12 @@ EXTRACT_ALL = NO EXTRACT_PRIVATE = NO +# If the EXTRACT_PRIV_VIRTUAL tag is set to YES, documented private virtual +# methods of a class will be included in the documentation. +# The default value is: NO. + +EXTRACT_PRIV_VIRTUAL = NO + # If the EXTRACT_PACKAGE tag is set to YES, all members with package or internal # scope will be included in the documentation. # The default value is: NO. @@ -480,6 +536,13 @@ EXTRACT_LOCAL_METHODS = NO EXTRACT_ANON_NSPACES = NO +# If this flag is set to YES, the name of an unnamed parameter in a declaration +# will be determined by the corresponding definition. By default unnamed +# parameters remain unnamed in the output. +# The default value is: YES. + +RESOLVE_UNNAMED_PARAMS = YES + # If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all # undocumented members inside documented classes or files. If set to NO these # members will be included in the various overviews, but no documentation @@ -497,8 +560,8 @@ HIDE_UNDOC_MEMBERS = NO HIDE_UNDOC_CLASSES = NO # If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend -# (class|struct|union) declarations. If set to NO, these declarations will be -# included in the documentation. +# declarations. If set to NO, these declarations will be included in the +# documentation. # The default value is: NO. HIDE_FRIEND_COMPOUNDS = NO @@ -517,11 +580,18 @@ HIDE_IN_BODY_DOCS = NO INTERNAL_DOCS = NO -# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file -# names in lower-case letters. If set to YES, upper-case letters are also -# allowed. This is useful if you have classes or files whose names only differ -# in case and if your file system supports case sensitive file names. Windows -# and Mac users are advised to set this option to NO. +# With the correct setting of option CASE_SENSE_NAMES doxygen will better be +# able to match the capabilities of the underlying filesystem. In case the +# filesystem is case sensitive (i.e. it supports files in the same directory +# whose names only differ in casing), the option must be set to YES to properly +# deal with such files in case they appear in the input. For filesystems that +# are not case sensitive the option should be be set to NO to properly deal with +# output files written for symbols that only differ in casing, such as for two +# classes, one named CLASS and the other named Class, and to also support +# references to files without having to specify the exact matching casing. On +# Windows (including Cygwin) and MacOS, users should typically set this option +# to NO, whereas on Linux or other Unix flavors it should typically be set to +# YES. # The default value is: system dependent. CASE_SENSE_NAMES = NO @@ -708,7 +778,7 @@ LAYOUT_FILE = # The CITE_BIB_FILES tag can be used to specify one or more bib files containing # the reference definitions. This must be a list of .bib files. The .bib # extension is automatically appended if omitted. This requires the bibtex tool -# to be installed. See also http://en.wikipedia.org/wiki/BibTeX for more info. +# to be installed. See also https://en.wikipedia.org/wiki/BibTeX for more info. # For LaTeX the style of the bibliography can be controlled using # LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the # search path. See also \cite for info how to create references. @@ -753,13 +823,17 @@ WARN_IF_DOC_ERROR = YES # This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that # are documented, but have no documentation for their parameters or return # value. If set to NO, doxygen will only warn about wrong or incomplete -# parameter documentation, but not about the absence of documentation. +# parameter documentation, but not about the absence of documentation. If +# EXTRACT_ALL is set to YES then this flag will automatically be disabled. # The default value is: NO. WARN_NO_PARAMDOC = NO # If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when -# a warning is encountered. +# a warning is encountered. If the WARN_AS_ERROR tag is set to FAIL_ON_WARNINGS +# then doxygen will continue running as if WARN_AS_ERROR tag is set to NO, but +# at the end of the doxygen process doxygen will return with a non-zero status. +# Possible values are: NO, YES and FAIL_ON_WARNINGS. # The default value is: NO. WARN_AS_ERROR = NO @@ -796,13 +870,13 @@ INPUT = README.md \ include \ lib \ resources \ - test + test # This tag can be used to specify the character encoding of the source files # that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses # libiconv (or the iconv built into libc) for the transcoding. See the libiconv -# documentation (see: http://www.gnu.org/software/libiconv) for the list of -# possible encodings. +# documentation (see: +# https://www.gnu.org/software/libiconv/) for the list of possible encodings. # The default value is: UTF-8. INPUT_ENCODING = UTF-8 @@ -815,11 +889,15 @@ INPUT_ENCODING = UTF-8 # need to set EXTENSION_MAPPING for the extension otherwise the files are not # read by doxygen. # +# Note the list of default checked file patterns might differ from the list of +# default file extension mappings. +# # If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp, # *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h, # *.hh, *.hxx, *.hpp, *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc, -# *.m, *.markdown, *.md, *.mm, *.dox, *.py, *.pyw, *.f90, *.f95, *.f03, *.f08, -# *.f, *.for, *.tcl, *.vhd, *.vhdl, *.ucf and *.qsf. +# *.m, *.markdown, *.md, *.mm, *.dox (to be provided as doxygen C comment), +# *.py, *.pyw, *.f90, *.f95, *.f03, *.f08, *.f18, *.f, *.for, *.vhd, *.vhdl, +# *.ucf, *.qsf and *.ice. FILE_PATTERNS = *.c \ *.cc \ @@ -1020,7 +1098,7 @@ INLINE_SOURCES = NO STRIP_CODE_COMMENTS = YES # If the REFERENCED_BY_RELATION tag is set to YES then for each documented -# function all documented functions referencing it will be listed. +# entity all documented functions referencing it will be listed. # The default value is: NO. REFERENCED_BY_RELATION = NO @@ -1052,12 +1130,12 @@ SOURCE_TOOLTIPS = YES # If the USE_HTAGS tag is set to YES then the references to source code will # point to the HTML generated by the htags(1) tool instead of doxygen built-in # source browser. The htags tool is part of GNU's global source tagging system -# (see http://www.gnu.org/software/global/global.html). You will need version +# (see https://www.gnu.org/software/global/global.html). You will need version # 4.8.6 or higher. # # To use it do the following: # - Install the latest version of global -# - Enable SOURCE_BROWSER and USE_HTAGS in the config file +# - Enable SOURCE_BROWSER and USE_HTAGS in the configuration file # - Make sure the INPUT points to the root of the source tree # - Run doxygen as normal # @@ -1080,16 +1158,22 @@ USE_HTAGS = NO VERBATIM_HEADERS = YES # If the CLANG_ASSISTED_PARSING tag is set to YES then doxygen will use the -# clang parser (see: http://clang.llvm.org/) for more accurate parsing at the -# cost of reduced performance. This can be particularly helpful with template -# rich C++ code for which doxygen's built-in parser lacks the necessary type -# information. +# clang parser (see: +# http://clang.llvm.org/) for more accurate parsing at the cost of reduced +# performance. This can be particularly helpful with template rich C++ code for +# which doxygen's built-in parser lacks the necessary type information. # Note: The availability of this option depends on whether or not doxygen was -# generated with the -Duse-libclang=ON option for CMake. +# generated with the -Duse_libclang=ON option for CMake. # The default value is: NO. CLANG_ASSISTED_PARSING = NO +# If clang assisted parsing is enabled and the CLANG_ADD_INC_PATHS tag is set to +# YES then doxygen will add the directory of each input to the include path. +# The default value is: YES. + +CLANG_ADD_INC_PATHS = YES + # If clang assisted parsing is enabled you can provide the compiler with command # line options that you would normally use when invoking the compiler. Note that # the include paths will already be set by doxygen for the files and directories @@ -1098,6 +1182,19 @@ CLANG_ASSISTED_PARSING = NO CLANG_OPTIONS = +# If clang assisted parsing is enabled you can provide the clang parser with the +# path to the directory containing a file called compile_commands.json. This +# file is the compilation database (see: +# http://clang.llvm.org/docs/HowToSetupToolingForLLVM.html) containing the +# options used when the source files were built. This is equivalent to +# specifying the -p option to a clang tool, such as clang-check. These options +# will then be passed to the parser. Any options specified with CLANG_OPTIONS +# will be added as well. +# Note: The availability of this option depends on whether or not doxygen was +# generated with the -Duse_libclang=ON option for CMake. + +CLANG_DATABASE_PATH = + #--------------------------------------------------------------------------- # Configuration options related to the alphabetical class index #--------------------------------------------------------------------------- @@ -1109,13 +1206,6 @@ CLANG_OPTIONS = ALPHABETICAL_INDEX = YES -# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in -# which the alphabetical index list will be split. -# Minimum value: 1, maximum value: 20, default value: 5. -# This tag requires that the tag ALPHABETICAL_INDEX is set to YES. - -COLS_IN_ALPHA_INDEX = 5 - # In case all classes in a project start with a common prefix, all classes will # be put under the same header in the alphabetical index. The IGNORE_PREFIX tag # can be used to specify a prefix (or a list of prefixes) that should be ignored @@ -1216,7 +1306,7 @@ HTML_EXTRA_FILES = # The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen # will adjust the colors in the style sheet and background images according to # this color. Hue is specified as an angle on a colorwheel, see -# http://en.wikipedia.org/wiki/Hue for more information. For instance the value +# https://en.wikipedia.org/wiki/Hue for more information. For instance the value # 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300 # purple, and 360 is red again. # Minimum value: 0, maximum value: 359, default value: 220. @@ -1252,6 +1342,17 @@ HTML_COLORSTYLE_GAMMA = 80 HTML_TIMESTAMP = NO +# If the HTML_DYNAMIC_MENUS tag is set to YES then the generated HTML +# documentation will contain a main index with vertical navigation menus that +# are dynamically created via JavaScript. If disabled, the navigation index will +# consists of multiple levels of tabs that are statically embedded in every HTML +# page. Disable this option to support browsers that do not have JavaScript, +# like the Qt help browser. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_DYNAMIC_MENUS = YES + # If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML # documentation will contain sections that can be hidden and shown after the # page has loaded. @@ -1275,13 +1376,14 @@ HTML_INDEX_NUM_ENTRIES = 100 # If the GENERATE_DOCSET tag is set to YES, additional index files will be # generated that can be used as input for Apple's Xcode 3 integrated development -# environment (see: http://developer.apple.com/tools/xcode/), introduced with -# OSX 10.5 (Leopard). To create a documentation set, doxygen will generate a -# Makefile in the HTML output directory. Running make will produce the docset in -# that directory and running make install will install the docset in +# environment (see: +# https://developer.apple.com/xcode/), introduced with OSX 10.5 (Leopard). To +# create a documentation set, doxygen will generate a Makefile in the HTML +# output directory. Running make will produce the docset in that directory and +# running make install will install the docset in # ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at -# startup. See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html -# for more information. +# startup. See https://developer.apple.com/library/archive/featuredarticles/Doxy +# genXcode/_index.html for more information. # The default value is: NO. # This tag requires that the tag GENERATE_HTML is set to YES. @@ -1320,8 +1422,8 @@ DOCSET_PUBLISHER_NAME = Publisher # If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three # additional HTML index files: index.hhp, index.hhc, and index.hhk. The # index.hhp is a project file that can be read by Microsoft's HTML Help Workshop -# (see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on -# Windows. +# (see: +# https://www.microsoft.com/en-us/download/details.aspx?id=21138) on Windows. # # The HTML Help Workshop contains a compiler that can convert all HTML output # generated by doxygen into a single compiled HTML file (.chm). Compiled HTML @@ -1351,7 +1453,7 @@ CHM_FILE = HHC_LOCATION = # The GENERATE_CHI flag controls if a separate .chi index file is generated -# (YES) or that it should be included in the master .chm file (NO). +# (YES) or that it should be included in the main .chm file (NO). # The default value is: NO. # This tag requires that the tag GENERATE_HTMLHELP is set to YES. @@ -1396,7 +1498,8 @@ QCH_FILE = # The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help # Project output. For more information please see Qt Help Project / Namespace -# (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#namespace). +# (see: +# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#namespace). # The default value is: org.doxygen.Project. # This tag requires that the tag GENERATE_QHP is set to YES. @@ -1404,8 +1507,8 @@ QHP_NAMESPACE = org.doxygen.Project # The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt # Help Project output. For more information please see Qt Help Project / Virtual -# Folders (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#virtual- -# folders). +# Folders (see: +# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#virtual-folders). # The default value is: doc. # This tag requires that the tag GENERATE_QHP is set to YES. @@ -1413,30 +1516,30 @@ QHP_VIRTUAL_FOLDER = doc # If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom # filter to add. For more information please see Qt Help Project / Custom -# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom- -# filters). +# Filters (see: +# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#custom-filters). # This tag requires that the tag GENERATE_QHP is set to YES. QHP_CUST_FILTER_NAME = # The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the # custom filter to add. For more information please see Qt Help Project / Custom -# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom- -# filters). +# Filters (see: +# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#custom-filters). # This tag requires that the tag GENERATE_QHP is set to YES. QHP_CUST_FILTER_ATTRS = # The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this # project's filter section matches. Qt Help Project / Filter Attributes (see: -# http://qt-project.org/doc/qt-4.8/qthelpproject.html#filter-attributes). +# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#filter-attributes). # This tag requires that the tag GENERATE_QHP is set to YES. QHP_SECT_FILTER_ATTRS = -# The QHG_LOCATION tag can be used to specify the location of Qt's -# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the -# generated .qhp file. +# The QHG_LOCATION tag can be used to specify the location (absolute path +# including file name) of Qt's qhelpgenerator. If non-empty doxygen will try to +# run qhelpgenerator on the generated .qhp file. # This tag requires that the tag GENERATE_QHP is set to YES. QHG_LOCATION = @@ -1513,6 +1616,17 @@ TREEVIEW_WIDTH = 250 EXT_LINKS_IN_WINDOW = NO +# If the HTML_FORMULA_FORMAT option is set to svg, doxygen will use the pdf2svg +# tool (see https://github.com/dawbarton/pdf2svg) or inkscape (see +# https://inkscape.org) to generate formulas as SVG images instead of PNGs for +# the HTML output. These images will generally look nicer at scaled resolutions. +# Possible values are: png (the default) and svg (looks nicer but requires the +# pdf2svg or inkscape tool). +# The default value is: png. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FORMULA_FORMAT = png + # Use this tag to change the font size of LaTeX formulas included as images in # the HTML documentation. When you change the font size after a successful # doxygen run you need to manually remove any form_*.png images from the HTML @@ -1522,7 +1636,7 @@ EXT_LINKS_IN_WINDOW = NO FORMULA_FONTSIZE = 10 -# Use the FORMULA_TRANPARENT tag to determine whether or not the images +# Use the FORMULA_TRANSPARENT tag to determine whether or not the images # generated for formulas are transparent PNGs. Transparent PNGs are not # supported properly for IE 6.0, but are supported on all modern browsers. # @@ -1533,8 +1647,14 @@ FORMULA_FONTSIZE = 10 FORMULA_TRANSPARENT = YES +# The FORMULA_MACROFILE can contain LaTeX \newcommand and \renewcommand commands +# to create new LaTeX commands to be used in formulas as building blocks. See +# the section "Including formulas" for details. + +FORMULA_MACROFILE = + # Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see -# http://www.mathjax.org) which uses client side Javascript for the rendering +# https://www.mathjax.org) which uses client side JavaScript for the rendering # instead of using pre-rendered bitmaps. Use this if you do not have LaTeX # installed or if you want to formulas look prettier in the HTML output. When # enabled you may also need to install MathJax separately and configure the path @@ -1546,7 +1666,7 @@ USE_MATHJAX = YES # When MathJax is enabled you can set the default output format to be used for # the MathJax output. See the MathJax site (see: -# http://docs.mathjax.org/en/latest/output.html) for more details. +# http://docs.mathjax.org/en/v2.7-latest/output.html) for more details. # Possible values are: HTML-CSS (which is slower, but has the best # compatibility), NativeMML (i.e. MathML) and SVG. # The default value is: HTML-CSS. @@ -1561,12 +1681,10 @@ MATHJAX_FORMAT = HTML-CSS # MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax # Content Delivery Network so you can quickly see the result without installing # MathJax. However, it is strongly recommended to install a local copy of -# MathJax from http://www.mathjax.org before deployment. -# The default value is: http://cdn.mathjax.org/mathjax/latest. +# MathJax from https://www.mathjax.org before deployment. +# The default value is: https://cdn.jsdelivr.net/npm/mathjax@2. # This tag requires that the tag USE_MATHJAX is set to YES. -# Note from Peizhang: must use https instead of http to be able to render math symbols with github.io - MATHJAX_RELPATH = https://cdn.mathjax.org/mathjax/latest # The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax @@ -1578,7 +1696,8 @@ MATHJAX_EXTENSIONS = TeX/AMSmath # The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces # of code that will be used on startup of the MathJax code. See the MathJax site -# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an +# (see: +# http://docs.mathjax.org/en/v2.7-latest/output.html) for more details. For an # example see the documentation. # This tag requires that the tag USE_MATHJAX is set to YES. @@ -1606,7 +1725,7 @@ MATHJAX_CODEFILE = SEARCHENGINE = YES # When the SERVER_BASED_SEARCH tag is enabled the search engine will be -# implemented using a web server instead of a web client using Javascript. There +# implemented using a web server instead of a web client using JavaScript. There # are two flavors of web server based searching depending on the EXTERNAL_SEARCH # setting. When disabled, doxygen will generate a PHP script for searching and # an index file used by the script. When EXTERNAL_SEARCH is enabled the indexing @@ -1625,7 +1744,8 @@ SERVER_BASED_SEARCH = NO # # Doxygen ships with an example indexer (doxyindexer) and search engine # (doxysearch.cgi) which are based on the open source search engine library -# Xapian (see: http://xapian.org/). +# Xapian (see: +# https://xapian.org/). # # See the section "External Indexing and Searching" for details. # The default value is: NO. @@ -1638,8 +1758,9 @@ EXTERNAL_SEARCH = NO # # Doxygen ships with an example indexer (doxyindexer) and search engine # (doxysearch.cgi) which are based on the open source search engine library -# Xapian (see: http://xapian.org/). See the section "External Indexing and -# Searching" for details. +# Xapian (see: +# https://xapian.org/). See the section "External Indexing and Searching" for +# details. # This tag requires that the tag SEARCHENGINE is set to YES. SEARCHENGINE_URL = @@ -1690,21 +1811,35 @@ LATEX_OUTPUT = latex # The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be # invoked. # -# Note that when enabling USE_PDFLATEX this option is only used for generating -# bitmaps for formulas in the HTML output, but not in the Makefile that is -# written to the output directory. -# The default file is: latex. +# Note that when not enabling USE_PDFLATEX the default is latex when enabling +# USE_PDFLATEX the default is pdflatex and when in the later case latex is +# chosen this is overwritten by pdflatex. For specific output languages the +# default can have been set differently, this depends on the implementation of +# the output language. # This tag requires that the tag GENERATE_LATEX is set to YES. LATEX_CMD_NAME = latex # The MAKEINDEX_CMD_NAME tag can be used to specify the command name to generate # index for LaTeX. +# Note: This tag is used in the Makefile / make.bat. +# See also: LATEX_MAKEINDEX_CMD for the part in the generated output file +# (.tex). # The default file is: makeindex. # This tag requires that the tag GENERATE_LATEX is set to YES. MAKEINDEX_CMD_NAME = makeindex +# The LATEX_MAKEINDEX_CMD tag can be used to specify the command name to +# generate index for LaTeX. In case there is no backslash (\) as first character +# it will be automatically added in the LaTeX code. +# Note: This tag is used in the generated output file (.tex). +# See also: MAKEINDEX_CMD_NAME for the part in the Makefile / make.bat. +# The default value is: makeindex. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +LATEX_MAKEINDEX_CMD = makeindex + # If the COMPACT_LATEX tag is set to YES, doxygen generates more compact LaTeX # documents. This may be useful for small projects and may help to save some # trees in general. @@ -1789,9 +1924,11 @@ LATEX_EXTRA_FILES = PDF_HYPERLINKS = YES -# If the USE_PDFLATEX tag is set to YES, doxygen will use pdflatex to generate -# the PDF file directly from the LaTeX files. Set this option to YES, to get a -# higher quality PDF documentation. +# If the USE_PDFLATEX tag is set to YES, doxygen will use the engine as +# specified with LATEX_CMD_NAME to generate the PDF file directly from the LaTeX +# files. Set this option to YES, to get a higher quality PDF documentation. +# +# See also section LATEX_CMD_NAME for selecting the engine. # The default value is: YES. # This tag requires that the tag GENERATE_LATEX is set to YES. @@ -1825,7 +1962,7 @@ LATEX_SOURCE_CODE = NO # The LATEX_BIB_STYLE tag can be used to specify the style to use for the # bibliography, e.g. plainnat, or ieeetr. See -# http://en.wikipedia.org/wiki/BibTeX and \cite for more info. +# https://en.wikipedia.org/wiki/BibTeX and \cite for more info. # The default value is: plain. # This tag requires that the tag GENERATE_LATEX is set to YES. @@ -1839,6 +1976,14 @@ LATEX_BIB_STYLE = plain LATEX_TIMESTAMP = NO +# The LATEX_EMOJI_DIRECTORY tag is used to specify the (relative or absolute) +# path from which the emoji images will be read. If a relative path is entered, +# it will be relative to the LATEX_OUTPUT directory. If left blank the +# LATEX_OUTPUT directory will be used. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +LATEX_EMOJI_DIRECTORY = + #--------------------------------------------------------------------------- # Configuration options related to the RTF output #--------------------------------------------------------------------------- @@ -1878,9 +2023,9 @@ COMPACT_RTF = NO RTF_HYPERLINKS = NO -# Load stylesheet definitions from file. Syntax is similar to doxygen's config -# file, i.e. a series of assignments. You only have to provide replacements, -# missing definitions are set to their default value. +# Load stylesheet definitions from file. Syntax is similar to doxygen's +# configuration file, i.e. a series of assignments. You only have to provide +# replacements, missing definitions are set to their default value. # # See also section "Doxygen usage" for information on how to generate the # default style sheet that doxygen normally uses. @@ -1889,8 +2034,8 @@ RTF_HYPERLINKS = NO RTF_STYLESHEET_FILE = # Set optional variables used in the generation of an RTF document. Syntax is -# similar to doxygen's config file. A template extensions file can be generated -# using doxygen -e rtf extensionFile. +# similar to doxygen's configuration file. A template extensions file can be +# generated using doxygen -e rtf extensionFile. # This tag requires that the tag GENERATE_RTF is set to YES. RTF_EXTENSIONS_FILE = @@ -1976,6 +2121,13 @@ XML_OUTPUT = xml XML_PROGRAMLISTING = YES +# If the XML_NS_MEMB_FILE_SCOPE tag is set to YES, doxygen will include +# namespace members in file scope as well, matching the HTML output. +# The default value is: NO. +# This tag requires that the tag GENERATE_XML is set to YES. + +XML_NS_MEMB_FILE_SCOPE = NO + #--------------------------------------------------------------------------- # Configuration options related to the DOCBOOK output #--------------------------------------------------------------------------- @@ -2008,9 +2160,9 @@ DOCBOOK_PROGRAMLISTING = NO #--------------------------------------------------------------------------- # If the GENERATE_AUTOGEN_DEF tag is set to YES, doxygen will generate an -# AutoGen Definitions (see http://autogen.sf.net) file that captures the -# structure of the code including all documentation. Note that this feature is -# still experimental and incomplete at the moment. +# AutoGen Definitions (see http://autogen.sourceforge.net/) file that captures +# the structure of the code including all documentation. Note that this feature +# is still experimental and incomplete at the moment. # The default value is: NO. GENERATE_AUTOGEN_DEF = NO @@ -2287,10 +2439,32 @@ UML_LOOK = NO # but if the number exceeds 15, the total amount of fields shown is limited to # 10. # Minimum value: 0, maximum value: 100, default value: 10. -# This tag requires that the tag HAVE_DOT is set to YES. +# This tag requires that the tag UML_LOOK is set to YES. UML_LIMIT_NUM_FIELDS = 10 +# If the DOT_UML_DETAILS tag is set to NO, doxygen will show attributes and +# methods without types and arguments in the UML graphs. If the DOT_UML_DETAILS +# tag is set to YES, doxygen will add type and arguments for attributes and +# methods in the UML graphs. If the DOT_UML_DETAILS tag is set to NONE, doxygen +# will not generate fields with class member information in the UML graphs. The +# class diagrams will look similar to the default class diagrams but using UML +# notation for the relationships. +# Possible values are: NO, YES and NONE. +# The default value is: NO. +# This tag requires that the tag UML_LOOK is set to YES. + +DOT_UML_DETAILS = NO + +# The DOT_WRAP_THRESHOLD tag can be used to set the maximum number of characters +# to display on a single line. If the actual line length exceeds this threshold +# significantly it will wrapped across multiple lines. Some heuristics are apply +# to avoid ugly line breaks. +# Minimum value: 0, maximum value: 1000, default value: 17. +# This tag requires that the tag HAVE_DOT is set to YES. + +DOT_WRAP_THRESHOLD = 17 + # If the TEMPLATE_RELATIONS tag is set to YES then the inheritance and # collaboration graphs will show the relations between templates and their # instances. @@ -2482,9 +2656,11 @@ DOT_MULTI_TARGETS = NO GENERATE_LEGEND = YES -# If the DOT_CLEANUP tag is set to YES, doxygen will remove the intermediate dot +# If the DOT_CLEANUP tag is set to YES, doxygen will remove the intermediate # files that are used to generate the various graphs. +# +# Note: This setting is not only used for dot files but also for msc and +# plantuml temporary files. # The default value is: YES. -# This tag requires that the tag HAVE_DOT is set to YES. DOT_CLEANUP = YES diff --git a/example/CMakeLists.txt b/example/CMakeLists.txt index 0d622b10..6033353f 100644 --- a/example/CMakeLists.txt +++ b/example/CMakeLists.txt @@ -22,17 +22,19 @@ set(EXAMPLE_LIST basics6_gripper_control basics7_auto_recovery basics8_update_robot_tool - intermediate6_robot_dynamics + basics9_logging_behavior + intermediate7_robot_dynamics ) # Additional examples for Linux and Mac if(CMAKE_HOST_UNIX) list(APPEND EXAMPLE_LIST intermediate1_realtime_joint_position_control - intermediate2_realtime_joint_torque_control - intermediate3_realtime_joint_floating - intermediate4_realtime_cartesian_pure_motion_control - intermediate5_realtime_cartesian_motion_force_control + intermediate2_realtime_joint_impedance_control + intermediate3_realtime_joint_torque_control + intermediate4_realtime_joint_floating + intermediate5_realtime_cartesian_pure_motion_control + intermediate6_realtime_cartesian_motion_force_control ) endif() diff --git a/example/basics1_display_robot_states.cpp b/example/basics1_display_robot_states.cpp index 54c22857..28ec7cea 100644 --- a/example/basics1_display_robot_states.cpp +++ b/example/basics1_display_robot_states.cpp @@ -2,26 +2,17 @@ * @example basics1_display_robot_states.cpp * This tutorial does the very first thing: check connection with the robot server and print * received robot states. - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ -#include -#include -#include +#include +#include +#include #include #include -/** @brief Print tutorial description */ -void PrintDescription() -{ - std::cout << "This tutorial does the very first thing: check connection with the robot server " - "and print received robot states." - << std::endl - << std::endl; -} - /** @brief Print program usage help */ void PrintHelp() { @@ -35,11 +26,11 @@ void PrintHelp() } /** @brief Print robot states data @ 1Hz */ -void printRobotStates(flexiv::Robot& robot, flexiv::Log& log) +void printRobotStates(flexiv::rdk::Robot& robot) { while (true) { // Print all robot states in JSON format using the built-in ostream operator overloading - log.Info("Current robot states:"); + spdlog::info("Current robot states:"); std::cout << robot.states() << std::endl; std::this_thread::sleep_for(std::chrono::seconds(1)); } @@ -49,11 +40,8 @@ int main(int argc, char* argv[]) { // Program Setup // ============================================================================================= - // Logger for printing message with timestamp and coloring - flexiv::Log log; - // Parse parameters - if (argc < 2 || flexiv::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { + if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { PrintHelp(); return 1; } @@ -61,48 +49,48 @@ int main(int argc, char* argv[]) std::string robot_sn = argv[1]; // Print description - log.Info("Tutorial description:"); - PrintDescription(); + spdlog::info( + ">>> Tutorial description <<<\nThis tutorial does the very first thing: check connection " + "with the robot server and print received robot states."); try { // RDK Initialization // ========================================================================================= // Instantiate robot interface - flexiv::Robot robot(robot_sn); + flexiv::rdk::Robot robot(robot_sn); // Clear fault on the connected robot if any if (robot.fault()) { - log.Warn("Fault occurred on the connected robot, trying to clear ..."); + spdlog::warn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault if (!robot.ClearFault()) { - log.Error("Fault cannot be cleared, exiting ..."); + spdlog::error("Fault cannot be cleared, exiting ..."); return 1; } - log.Info("Fault on the connected robot is cleared"); + spdlog::info("Fault on the connected robot is cleared"); } // Enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ..."); + spdlog::info("Enabling robot ..."); robot.Enable(); // Wait for the robot to become operational while (!robot.operational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); } - log.Info("Robot is now operational"); + spdlog::info("Robot is now operational"); // Print States // ========================================================================================= // Use std::thread to do scheduling so that this example can run on all OS, since not all OS - // support flexiv::Scheduler - std::thread low_priority_thread( - std::bind(printRobotStates, std::ref(robot), std::ref(log))); + // support flexiv::rdk::Scheduler + std::thread low_priority_thread(std::bind(printRobotStates, std::ref(robot))); // Properly exit thread low_priority_thread.join(); } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); return 1; } diff --git a/example/basics2_clear_fault.cpp b/example/basics2_clear_fault.cpp index 4b3ece05..80d79d09 100644 --- a/example/basics2_clear_fault.cpp +++ b/example/basics2_clear_fault.cpp @@ -1,28 +1,18 @@ /** * @example basics2_clear_fault.cpp - * This tutorial clears minor faults from the robot server if any. Note that critical faults cannot - * be cleared, see RDK manual for more details. - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * This tutorial clears minor or critical faults, if any, of the connected robot. + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ -#include -#include -#include +#include +#include +#include #include #include #include -/** @brief Print tutorial description */ -void PrintDescription() -{ - std::cout << "This tutorial clears minor faults from the robot server if any. Note that " - "critical faults cannot be cleared, see RDK manual for more details." - << std::endl - << std::endl; -} - /** @brief Print program usage help */ void PrintHelp() { @@ -39,11 +29,8 @@ int main(int argc, char* argv[]) { // Program Setup // ============================================================================================= - // Logger for printing message with timestamp and coloring - flexiv::Log log; - // Parse parameters - if (argc < 2 || flexiv::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { + if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { PrintHelp(); return 1; } @@ -51,31 +38,32 @@ int main(int argc, char* argv[]) std::string robot_sn = argv[1]; // Print description - log.Info("Tutorial description:"); - PrintDescription(); + spdlog::info( + ">>> Tutorial description <<<\nThis tutorial clears minor or critical faults, if any, of " + "the connected robot."); try { // RDK Initialization // ========================================================================================= // Instantiate robot interface - flexiv::Robot robot(robot_sn); + flexiv::rdk::Robot robot(robot_sn); // Fault Clearing // ========================================================================================= // Clear fault on the connected robot if any if (robot.fault()) { - log.Warn("Fault occurred on the connected robot, trying to clear ..."); + spdlog::warn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault if (!robot.ClearFault()) { - log.Error("Fault cannot be cleared, exiting ..."); + spdlog::error("Fault cannot be cleared, exiting ..."); return 1; } - log.Info("Fault on the connected robot is cleared"); + spdlog::info("Fault on the connected robot is cleared"); } else { - log.Info("No fault on the connected robot"); + spdlog::info("No fault on the connected robot"); } } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); return 1; } diff --git a/example/basics3_primitive_execution.cpp b/example/basics3_primitive_execution.cpp index 3906f582..7c62857b 100644 --- a/example/basics3_primitive_execution.cpp +++ b/example/basics3_primitive_execution.cpp @@ -2,27 +2,17 @@ * @example basics3_primitive_execution.cpp * This tutorial executes several basic robot primitives (unit skills). For detailed documentation * on all available primitives, please see [Flexiv Primitives](https://www.flexiv.com/primitives/). - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ -#include -#include -#include +#include +#include +#include #include #include -/** @brief Print tutorial description */ -void PrintDescription() -{ - std::cout << "This tutorial executes several basic robot primitives (unit skills). For " - "detailed documentation on all available primitives, please see [Flexiv " - "Primitives](https://www.flexiv.com/primitives/)." - << std::endl - << std::endl; -} - /** @brief Print program usage help */ void PrintHelp() { @@ -39,11 +29,8 @@ int main(int argc, char* argv[]) { // Program Setup // ============================================================================================= - // Logger for printing message with timestamp and coloring - flexiv::Log log; - // Parse parameters - if (argc < 2 || flexiv::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { + if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { PrintHelp(); return 1; } @@ -51,46 +38,48 @@ int main(int argc, char* argv[]) std::string robot_sn = argv[1]; // Print description - log.Info("Tutorial description:"); - PrintDescription(); + spdlog::info( + ">>> Tutorial description <<<\nThis tutorial executes several basic robot primitives (unit " + "skills). For detailed documentation on all available primitives, please see [Flexiv " + "Primitives](https://www.flexiv.com/primitives/)."); try { // RDK Initialization // ========================================================================================= // Instantiate robot interface - flexiv::Robot robot(robot_sn); + flexiv::rdk::Robot robot(robot_sn); // Clear fault on the connected robot if any if (robot.fault()) { - log.Warn("Fault occurred on the connected robot, trying to clear ..."); + spdlog::warn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault if (!robot.ClearFault()) { - log.Error("Fault cannot be cleared, exiting ..."); + spdlog::error("Fault cannot be cleared, exiting ..."); return 1; } - log.Info("Fault on the connected robot is cleared"); + spdlog::info("Fault on the connected robot is cleared"); } // Enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ..."); + spdlog::info("Enabling robot ..."); robot.Enable(); // Wait for the robot to become operational while (!robot.operational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); } - log.Info("Robot is now operational"); + spdlog::info("Robot is now operational"); // Execute Primitives // ========================================================================================= // Switch to primitive execution mode - robot.SwitchMode(flexiv::Mode::NRT_PRIMITIVE_EXECUTION); + robot.SwitchMode(flexiv::rdk::Mode::NRT_PRIMITIVE_EXECUTION); // (1) Go to home pose // ----------------------------------------------------------------------------------------- // All parameters of the "Home" primitive are optional, thus we can skip the parameters and // the default values will be used - log.Info("Executing primitive: Home"); + spdlog::info("Executing primitive: Home"); // Send command to robot robot.ExecutePrimitive("Home()"); @@ -103,13 +92,14 @@ int main(int argc, char* argv[]) // (2) Move robot joints to target positions // ----------------------------------------------------------------------------------------- // The required parameter takes in 7 target joint positions. Unit: degrees - log.Info("Executing primitive: MoveJ"); + spdlog::info("Executing primitive: MoveJ"); // Send command to robot robot.ExecutePrimitive("MoveJ(target=30 -45 0 90 0 40 30)"); // Wait for reached target - while (flexiv::utility::ParsePtStates(robot.primitive_states(), "reachedTarget") != "1") { + while ( + flexiv::rdk::utility::ParsePtStates(robot.primitive_states(), "reachedTarget") != "1") { std::this_thread::sleep_for(std::chrono::seconds(1)); } @@ -122,21 +112,22 @@ int main(int argc, char* argv[]) // Optional parameter: // waypoints: waypoints to pass before reaching final target // (same format as above, but can repeat for number of waypoints) - // maxVel: maximum TCP linear velocity + // vel: TCP linear velocity // Unit: m/s // NOTE: The rotations use Euler ZYX convention, rot_x means Euler ZYX angle around X axis - log.Info("Executing primitive: MoveL"); + spdlog::info("Executing primitive: MoveL"); // Send command to robot robot.ExecutePrimitive( "MoveL(target=0.65 -0.3 0.2 180 0 180 WORLD WORLD_ORIGIN,waypoints=0.45 0.1 0.2 180 0 " - "180 WORLD WORLD_ORIGIN : 0.45 -0.3 0.2 180 0 180 WORLD WORLD_ORIGIN, maxVel=0.2)"); + "180 WORLD WORLD_ORIGIN : 0.45 -0.3 0.2 180 0 180 WORLD WORLD_ORIGIN, vel=0.2)"); // The [Move] series primitive won't terminate itself, so we determine if the robot has // reached target location by checking the primitive state "reachedTarget = 1" in the list // of current primitive states, and terminate the current primitive manually by sending a // new primitive command. - while (flexiv::utility::ParsePtStates(robot.primitive_states(), "reachedTarget") != "1") { + while ( + flexiv::rdk::utility::ParsePtStates(robot.primitive_states(), "reachedTarget") != "1") { std::this_thread::sleep_for(std::chrono::seconds(1)); } @@ -144,20 +135,22 @@ int main(int argc, char* argv[]) // ----------------------------------------------------------------------------------------- // In this example the reference frame is changed from WORLD::WORLD_ORIGIN to TRAJ::START, // which represents the current TCP frame - log.Info("Executing primitive: MoveL"); + spdlog::info("Executing primitive: MoveL"); // Example to convert target quaternion [w,x,y,z] to Euler ZYX using utility functions std::array targetQuat = {0.9185587, 0.1767767, 0.3061862, 0.1767767}; // ZYX = [30, 30, 30] degrees - auto targetEulerDeg = flexiv::utility::Rad2Deg(flexiv::utility::Quat2EulerZYX(targetQuat)); + auto targetEulerDeg + = flexiv::rdk::utility::Rad2Deg(flexiv::rdk::utility::Quat2EulerZYX(targetQuat)); // Send command to robot. This motion will hold current TCP position and only do TCP // rotation - robot.ExecutePrimitive( - "MoveL(target=0.0 0.0 0.0 " + flexiv::utility::Arr2Str(targetEulerDeg) + "TRAJ START)"); + robot.ExecutePrimitive("MoveL(target=0.0 0.0 0.0 " + + flexiv::rdk::utility::Arr2Str(targetEulerDeg) + "TRAJ START)"); // Wait for reached target - while (flexiv::utility::ParsePtStates(robot.primitive_states(), "reachedTarget") != "1") { + while ( + flexiv::rdk::utility::ParsePtStates(robot.primitive_states(), "reachedTarget") != "1") { std::this_thread::sleep_for(std::chrono::seconds(1)); } @@ -165,7 +158,7 @@ int main(int argc, char* argv[]) robot.Stop(); } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); return 1; } diff --git a/example/basics4_plan_execution.cpp b/example/basics4_plan_execution.cpp index 417da642..9313ab62 100644 --- a/example/basics4_plan_execution.cpp +++ b/example/basics4_plan_execution.cpp @@ -4,29 +4,17 @@ * pre-written script to execute a series of robot primitives with pre-defined transition conditions * between 2 adjacent primitives. Users can use Flexiv Elements to compose their own plan and assign * to the robot, which will appear in the plan list. - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ -#include -#include -#include +#include +#include +#include #include #include -/** @brief Print tutorial description */ -void PrintDescription() -{ - std::cout << "This tutorial executes a plan selected by the user from a list of available " - "plans. A plan is a pre-written script to execute a series of robot primitives " - "with pre-defined transition conditions between 2 adjacent primitives. Users can " - "use Flexiv Elements to compose their own plan and assign to the robot, which " - "will appear in the plan list." - << std::endl - << std::endl; -} - /** @brief Print program usage help */ void PrintHelp() { @@ -43,11 +31,8 @@ int main(int argc, char* argv[]) { // Program Setup // ============================================================================================= - // Logger for printing message with timestamp and coloring - flexiv::Log log; - // Parse parameters - if (argc < 2 || flexiv::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { + if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { PrintHelp(); return 1; } @@ -55,40 +40,44 @@ int main(int argc, char* argv[]) std::string robot_sn = argv[1]; // Print description - log.Info("Tutorial description:"); - PrintDescription(); + spdlog::info( + ">>> Tutorial description <<<\nThis tutorial executes a plan selected by the user from a " + "list of available plans. A plan is a pre-written script to execute a series of robot " + "primitives with pre-defined transition conditions between 2 adjacent primitives. Users " + "can use Flexiv Elements to compose their own plan and assign to the robot, which " + "will appear in the plan list."); try { // RDK Initialization // ========================================================================================= // Instantiate robot interface - flexiv::Robot robot(robot_sn); + flexiv::rdk::Robot robot(robot_sn); // Clear fault on the connected robot if any if (robot.fault()) { - log.Warn("Fault occurred on the connected robot, trying to clear ..."); + spdlog::warn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault if (!robot.ClearFault()) { - log.Error("Fault cannot be cleared, exiting ..."); + spdlog::error("Fault cannot be cleared, exiting ..."); return 1; } - log.Info("Fault on the connected robot is cleared"); + spdlog::info("Fault on the connected robot is cleared"); } // Enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ..."); + spdlog::info("Enabling robot ..."); robot.Enable(); // Wait for the robot to become operational while (!robot.operational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); } - log.Info("Robot is now operational"); + spdlog::info("Robot is now operational"); // Execute Plans // ========================================================================================= // Switch to plan execution mode - robot.SwitchMode(flexiv::Mode::NRT_PLAN_EXECUTION); + robot.SwitchMode(flexiv::rdk::Mode::NRT_PLAN_EXECUTION); while (true) { // Monitor fault on the connected robot @@ -97,7 +86,7 @@ int main(int argc, char* argv[]) } // Get user input - log.Info("Choose an action:"); + spdlog::info("Choose an action:"); std::cout << "[1] Show available plans" << std::endl; std::cout << "[2] Execute a plan by index" << std::endl; std::cout << "[3] Execute a plan by name" << std::endl; @@ -117,7 +106,7 @@ int main(int argc, char* argv[]) } break; // Execute plan by index case 2: { - log.Info("Enter plan index to execute:"); + spdlog::info("Enter plan index to execute:"); int index; std::cin >> index; // Allow the plan to continue its execution even if the RDK program is closed or @@ -126,14 +115,14 @@ int main(int argc, char* argv[]) // Print plan info while the current plan is running while (robot.busy()) { - log.Info("==============================================="); + spdlog::info("Current plan info:"); std::cout << robot.plan_info() << std::endl; std::this_thread::sleep_for(std::chrono::seconds(1)); } } break; // Execute plan by name case 3: { - log.Info("Enter plan name to execute:"); + spdlog::info("Enter plan name to execute:"); std::string name; std::cin >> name; // Allow the plan to continue its execution even if the RDK program is closed or @@ -142,19 +131,19 @@ int main(int argc, char* argv[]) // Print plan info while the current plan is running while (robot.busy()) { - log.Info("==============================================="); + spdlog::info("Current plan info:"); std::cout << robot.plan_info() << std::endl; std::this_thread::sleep_for(std::chrono::seconds(1)); } } break; default: - log.Warn("Invalid input"); + spdlog::warn("Invalid input"); break; } } } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); return 1; } diff --git a/example/basics5_zero_force_torque_sensors.cpp b/example/basics5_zero_force_torque_sensors.cpp index 1ab5c0e2..92a57361 100644 --- a/example/basics5_zero_force_torque_sensors.cpp +++ b/example/basics5_zero_force_torque_sensors.cpp @@ -2,27 +2,17 @@ * @example basics5_zero_force_torque_sensors.cpp * This tutorial zeros the robot's force and torque sensors, which is a recommended (but not * mandatory) step before any operations that require accurate force/torque measurement. - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ -#include -#include -#include +#include +#include +#include #include #include -/** @brief Print tutorial description */ -void PrintDescription() -{ - std::cout << "This tutorial zeros the robot's force and torque sensors, which is a recommended " - "(but not mandatory) step before any operations that require accurate " - "force/torque measurement." - << std::endl - << std::endl; -} - /** @brief Print program usage help */ void PrintHelp() { @@ -39,11 +29,8 @@ int main(int argc, char* argv[]) { // Program Setup // ============================================================================================= - // Logger for printing message with timestamp and coloring - flexiv::Log log; - // Parse parameters - if (argc < 2 || flexiv::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { + if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { PrintHelp(); return 1; } @@ -51,62 +38,67 @@ int main(int argc, char* argv[]) std::string robot_sn = argv[1]; // Print description - log.Info("Tutorial description:"); - PrintDescription(); + spdlog::info( + ">>> Tutorial description <<<\nThis tutorial zeros the robot's force and torque sensors, " + "which is a recommended (but not mandatory) step before any operations that require " + "accurate force/torque measurement."); try { // RDK Initialization // ========================================================================================= // Instantiate robot interface - flexiv::Robot robot(robot_sn); + flexiv::rdk::Robot robot(robot_sn); // Clear fault on the connected robot if any if (robot.fault()) { - log.Warn("Fault occurred on the connected robot, trying to clear ..."); + spdlog::warn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault if (!robot.ClearFault()) { - log.Error("Fault cannot be cleared, exiting ..."); + spdlog::error("Fault cannot be cleared, exiting ..."); return 1; } - log.Info("Fault on the connected robot is cleared"); + spdlog::info("Fault on the connected robot is cleared"); } // Enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ..."); + spdlog::info("Enabling robot ..."); robot.Enable(); // Wait for the robot to become operational while (!robot.operational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); } - log.Info("Robot is now operational"); + spdlog::info("Robot is now operational"); // Zero Sensors // ========================================================================================= // Get and print the current TCP force/moment readings - log.Info("TCP force and moment reading in base frame BEFORE sensor zeroing: " - + flexiv::utility::Arr2Str(robot.states().ext_wrench_in_world) + "[N][Nm]"); + spdlog::info("TCP force and moment reading in base frame BEFORE sensor zeroing: " + + flexiv::rdk::utility::Arr2Str(robot.states().ext_wrench_in_world) + + "[N][Nm]"); // Run the "ZeroFTSensor" primitive to automatically zero force and torque sensors - robot.SwitchMode(flexiv::Mode::NRT_PRIMITIVE_EXECUTION); + robot.SwitchMode(flexiv::rdk::Mode::NRT_PRIMITIVE_EXECUTION); robot.ExecutePrimitive("ZeroFTSensor()"); // WARNING: during the process, the robot must not contact anything, otherwise the result // will be inaccurate and affect following operations - log.Warn("Zeroing force/torque sensors, make sure nothing is in contact with the robot"); + spdlog::warn( + "Zeroing force/torque sensors, make sure nothing is in contact with the robot"); // Wait for primitive completion while (robot.busy()) { std::this_thread::sleep_for(std::chrono::seconds(1)); } - log.Info("Sensor zeroing complete"); + spdlog::info("Sensor zeroing complete"); // Get and print the current TCP force/moment readings - log.Info("TCP force and moment reading in base frame AFTER sensor zeroing: " - + flexiv::utility::Arr2Str(robot.states().ext_wrench_in_world) + "[N][Nm]"); + spdlog::info("TCP force and moment reading in base frame AFTER sensor zeroing: " + + flexiv::rdk::utility::Arr2Str(robot.states().ext_wrench_in_world) + + "[N][Nm]"); } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); return 1; } diff --git a/example/basics6_gripper_control.cpp b/example/basics6_gripper_control.cpp index 71986614..fce2a7f0 100644 --- a/example/basics6_gripper_control.cpp +++ b/example/basics6_gripper_control.cpp @@ -1,14 +1,14 @@ /** * @example basics6_gripper_control.cpp - * This tutorial does position and force control (if available) for grippers supported by Flexiv. - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * This tutorial does position and force (if available) control of grippers supported by Flexiv. + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -20,15 +20,6 @@ namespace { std::atomic g_finished = {false}; } -/** @brief Print tutorial description */ -void PrintDescription() -{ - std::cout << "This tutorial does position and force control (if available) for grippers " - "supported by Flexiv." - << std::endl - << std::endl; -} - /** @brief Print program usage help */ void PrintHelp() { @@ -42,11 +33,11 @@ void PrintHelp() } /** @brief Print gripper states data @ 1Hz */ -void PrintGripperStates(flexiv::Gripper& gripper, flexiv::Log& log) +void PrintGripperStates(flexiv::rdk::Gripper& gripper) { while (!g_finished) { // Print all gripper states in JSON format using the built-in ostream operator overloading - log.Info("Current gripper states:"); + spdlog::info("Current gripper states:"); std::cout << gripper.states() << std::endl; std::cout << "moving: " << gripper.moving() << std::endl; std::this_thread::sleep_for(std::chrono::seconds(1)); @@ -57,11 +48,8 @@ int main(int argc, char* argv[]) { // Program Setup // ============================================================================================= - // Logger for printing message with timestamp and coloring - flexiv::Log log; - // Parse parameters - if (argc < 2 || flexiv::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { + if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { PrintHelp(); return 1; } @@ -69,83 +57,84 @@ int main(int argc, char* argv[]) std::string robot_sn = argv[1]; // Print description - log.Info("Tutorial description:"); - PrintDescription(); + spdlog::info( + ">>> Tutorial description <<<\nThis tutorial does position and force (if available) " + "control of grippers supported by Flexiv."); try { // RDK Initialization // ========================================================================================= // Instantiate robot interface - flexiv::Robot robot(robot_sn); + flexiv::rdk::Robot robot(robot_sn); // Clear fault on the connected robot if any if (robot.fault()) { - log.Warn("Fault occurred on the connected robot, trying to clear ..."); + spdlog::warn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault if (!robot.ClearFault()) { - log.Error("Fault cannot be cleared, exiting ..."); + spdlog::error("Fault cannot be cleared, exiting ..."); return 1; } - log.Info("Fault on the connected robot is cleared"); + spdlog::info("Fault on the connected robot is cleared"); } // Enable the robot, make sure the E-Stop is released before enabling - log.Info("Enabling robot ..."); + spdlog::info("Enabling robot ..."); robot.Enable(); // Wait for the robot to become operational while (!robot.operational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); } - log.Info("Robot is now operational"); + spdlog::info("Robot is now operational"); // Gripper Control // ========================================================================================= // Gripper control is not available if the robot is in IDLE mode, so switch to some mode // other than IDLE - robot.SwitchMode(flexiv::Mode::NRT_PLAN_EXECUTION); + robot.SwitchMode(flexiv::rdk::Mode::NRT_PLAN_EXECUTION); robot.ExecutePlan("PLAN-Home"); std::this_thread::sleep_for(std::chrono::seconds(1)); // Instantiate gripper control interface - flexiv::Gripper gripper(robot); + flexiv::rdk::Gripper gripper(robot); // Manually initialize the gripper, not all grippers need this step - log.Info("Initializing gripper, this process takes about 10 seconds ..."); + spdlog::info("Initializing gripper, this process takes about 10 seconds ..."); gripper.Init(); - log.Info("Initialization complete"); + spdlog::info("Initialization complete"); // Thread for printing gripper states - std::thread print_thread(PrintGripperStates, std::ref(gripper), std::ref(log)); + std::thread print_thread(PrintGripperStates, std::ref(gripper)); // Position control - log.Info("Closing gripper"); + spdlog::info("Closing gripper"); gripper.Move(0.01, 0.1, 20); std::this_thread::sleep_for(std::chrono::seconds(2)); - log.Info("Opening gripper"); + spdlog::info("Opening gripper"); gripper.Move(0.09, 0.1, 20); std::this_thread::sleep_for(std::chrono::seconds(2)); // Stop - log.Info("Closing gripper"); + spdlog::info("Closing gripper"); gripper.Move(0.01, 0.1, 20); std::this_thread::sleep_for(std::chrono::milliseconds(500)); - log.Info("Stopping gripper"); + spdlog::info("Stopping gripper"); gripper.Stop(); std::this_thread::sleep_for(std::chrono::seconds(2)); - log.Info("Closing gripper"); + spdlog::info("Closing gripper"); gripper.Move(0.01, 0.1, 20); std::this_thread::sleep_for(std::chrono::seconds(2)); - log.Info("Opening gripper"); + spdlog::info("Opening gripper"); gripper.Move(0.09, 0.1, 20); std::this_thread::sleep_for(std::chrono::milliseconds(500)); - log.Info("Stopping gripper"); + spdlog::info("Stopping gripper"); gripper.Stop(); std::this_thread::sleep_for(std::chrono::seconds(2)); // Force control, if available (sensed force is not zero) if (fabs(gripper.states().force) > std::numeric_limits::epsilon()) { - log.Info("Gripper running zero force control"); + spdlog::info("Gripper running zero force control"); gripper.Grasp(0); // Exit after 10 seconds std::this_thread::sleep_for(std::chrono::seconds(10)); @@ -154,11 +143,11 @@ int main(int argc, char* argv[]) // Finished, exit all threads gripper.Stop(); g_finished = true; - log.Info("Program finished"); + spdlog::info("Program finished"); print_thread.join(); } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); return 1; } diff --git a/example/basics7_auto_recovery.cpp b/example/basics7_auto_recovery.cpp index 0edef7a7..0c609177 100644 --- a/example/basics7_auto_recovery.cpp +++ b/example/basics7_auto_recovery.cpp @@ -1,29 +1,19 @@ /** * @example basics7_auto_recovery.cpp * This tutorial runs an automatic recovery process if the robot's safety system is in recovery - * state. See flexiv::Robot::recovery() and RDK manual for more details. - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * state. See flexiv::rdk::Robot::recovery() and RDK manual for more details. + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ -#include -#include -#include +#include +#include +#include #include #include #include -/** @brief Print tutorial description */ -void PrintDescription() -{ - std::cout - << "This tutorial runs an automatic recovery process if the robot's safety system is in " - "recovery state. See flexiv::Robot::recovery() and RDK manual for more details." - << std::endl - << std::endl; -} - /** @brief Print program usage help */ void PrintHelp() { @@ -40,11 +30,8 @@ int main(int argc, char* argv[]) { // Program Setup // ============================================================================================= - // Logger for printing message with timestamp and coloring - flexiv::Log log; - // Parse parameters - if (argc < 2 || flexiv::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { + if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { PrintHelp(); return 1; } @@ -52,17 +39,19 @@ int main(int argc, char* argv[]) std::string robot_sn = argv[1]; // Print description - log.Info("Tutorial description:"); - PrintDescription(); + spdlog::info( + ">>> Tutorial description <<<\nThis tutorial runs an automatic recovery process if the " + "robot's safety system is in recovery state. See flexiv::rdk::Robot::recovery() and RDK " + "manual for more details."); try { // RDK Initialization // ========================================================================================= // Instantiate robot interface - flexiv::Robot robot(robot_sn); + flexiv::rdk::Robot robot(robot_sn); // Enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ..."); + spdlog::info("Enabling robot ..."); robot.Enable(); // Run Auto-recovery @@ -78,10 +67,10 @@ int main(int argc, char* argv[]) } // Otherwise the system is normal, do nothing else { - log.Info("Robot system is not in recovery state, nothing to be done, exiting ..."); + spdlog::info("Robot system is not in recovery state, nothing to be done, exiting ..."); } } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); return 1; } diff --git a/example/basics8_update_robot_tool.cpp b/example/basics8_update_robot_tool.cpp index dc18bd3f..e3cab958 100644 --- a/example/basics8_update_robot_tool.cpp +++ b/example/basics8_update_robot_tool.cpp @@ -3,30 +3,19 @@ * This tutorial shows how to online update and interact with the robot tools. All changes made to * the robot tool system will take effect immediately without needing to reboot. However, the robot * must be put into IDLE mode when making these changes. - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include -/** @brief Print tutorial description */ -void PrintDescription() -{ - std::cout - << "This tutorial shows how to online update and interact with the robot tools. All " - "changes made to the robot tool system will take effect immediately without needing to " - "reboot. However, the robot must be put into IDLE mode when making these changes." - << std::endl - << std::endl; -} - /** @brief Print program usage help */ void PrintHelp() { @@ -43,11 +32,8 @@ int main(int argc, char* argv[]) { // Program Setup // ============================================================================================= - // Logger for printing message with timestamp and coloring - flexiv::Log log; - // Parse parameters - if (argc < 2 || flexiv::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { + if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { PrintHelp(); return 1; } @@ -55,46 +41,49 @@ int main(int argc, char* argv[]) std::string robot_sn = argv[1]; // Print description - log.Info("Tutorial description:"); - PrintDescription(); + spdlog::info( + ">>> Tutorial description <<<\nThis tutorial shows how to online update and interact with " + "the robot tools. All changes made to the robot tool system will take effect immediately " + "without needing to reboot. However, the robot must be put into IDLE mode when making " + "these changes."); try { // RDK Initialization // ========================================================================================= // Instantiate robot interface - flexiv::Robot robot(robot_sn); + flexiv::rdk::Robot robot(robot_sn); // Clear fault on the connected robot if any if (robot.fault()) { - log.Warn("Fault occurred on the connected robot, trying to clear ..."); + spdlog::warn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault if (!robot.ClearFault()) { - log.Error("Fault cannot be cleared, exiting ..."); + spdlog::error("Fault cannot be cleared, exiting ..."); return 1; } - log.Info("Fault on the connected robot is cleared"); + spdlog::info("Fault on the connected robot is cleared"); } // Enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ..."); + spdlog::info("Enabling robot ..."); robot.Enable(); // Wait for the robot to become operational while (!robot.operational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); } - log.Info("Robot is now operational"); + spdlog::info("Robot is now operational"); // Update Robot Tool // ========================================================================================= // Make sure the robot is in IDLE mode - robot.SwitchMode(flexiv::Mode::IDLE); + robot.SwitchMode(flexiv::rdk::Mode::IDLE); // Instantiate tool interface - flexiv::Tool tool(robot); + flexiv::rdk::Tool tool(robot); // Get and print a list of already configured tools currently in the robot's tools pool - log.Info("All configured tools:"); + spdlog::info("All configured tools:"); auto tool_list = tool.list(); for (size_t i = 0; i < tool_list.size(); i++) { std::cout << "[" << i << "] " << tool_list[i] << std::endl; @@ -102,11 +91,11 @@ int main(int argc, char* argv[]) std::cout << std::endl; // Get and print the current active tool - log.Info("Current active tool: " + tool.name()); + spdlog::info("Current active tool: {}", tool.name()); // Set name and parameters for a new tool std::string new_tool_name = "ExampleTool1"; - flexiv::ToolParams new_tool_params; + flexiv::rdk::ToolParams new_tool_params; new_tool_params.mass = 0.9; new_tool_params.CoM = {0.0, 0.0, 0.057}; new_tool_params.inertia = {2.768e-03, 3.149e-03, 5.64e-04, 0.0, 0.0, 0.0}; @@ -115,19 +104,19 @@ int main(int argc, char* argv[]) // If there's already a tool with the same name in the robot's tools pool, then remove it // first, because duplicate tool names are not allowed if (tool.exist(new_tool_name)) { - log.Warn( - "Tool with the same name [" + new_tool_name + "] already exists, removing it now"); + spdlog::warn( + "Tool with the same name [{}] already exists, removing it now", new_tool_name); // Switch to other tool or no tool (Flange) before removing the current tool tool.Switch("Flange"); tool.Remove(new_tool_name); } // Add the new tool - log.Info("Adding new tool [" + new_tool_name + "] to the robot"); + spdlog::info("Adding new tool [{}] to the robot", new_tool_name); tool.Add(new_tool_name, new_tool_params); // Get and print the tools list again, the new tool should appear at the end - log.Info("All configured tools:"); + spdlog::info("All configured tools:"); tool_list = tool.list(); for (size_t i = 0; i < tool_list.size(); i++) { std::cout << "[" << i << "] " << tool_list[i] << std::endl; @@ -135,24 +124,24 @@ int main(int argc, char* argv[]) std::cout << std::endl; // Switch to the newly added tool, i.e. set it as the active tool - log.Info("Switching to tool [" + new_tool_name + "]"); + spdlog::info("Switching to tool [{}]", new_tool_name); tool.Switch(new_tool_name); // Get and print the current active tool again, should be the new tool - log.Info("Current active tool: " + tool.name()); + spdlog::info("Current active tool: {}", tool.name()); // Switch to other tool or no tool (Flange) before removing the current tool tool.Switch("Flange"); // Clean up by removing the new tool std::this_thread::sleep_for(std::chrono::seconds(2)); - log.Info("Removing tool [" + new_tool_name + "]"); + spdlog::info("Removing tool [{}]", new_tool_name); tool.Remove(new_tool_name); - log.Info("Program finished"); + spdlog::info("Program finished"); } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); return 1; } diff --git a/example/basics9_logging_behavior.cpp b/example/basics9_logging_behavior.cpp new file mode 100644 index 00000000..6be856fd --- /dev/null +++ b/example/basics9_logging_behavior.cpp @@ -0,0 +1,96 @@ +/** + * @example basics9_logging_behavior.cpp + * This tutorial shows how to change the logging behaviors of RDK client. + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. + * @author Flexiv + */ + +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace { +constexpr char kDefaultLogPattern[] = "[%Y-%m-%d %H:%M:%S.%e] [%^%l%$] %v"; +} + +/** @brief Print program usage help */ +void PrintHelp() +{ + // clang-format off + std::cout << "Required arguments: [robot SN]" << std::endl; + std::cout << " robot SN: Serial number of the robot to connect to. " + "Remove any space, for example: Rizon4s-123456" << std::endl; + std::cout << "Optional arguments: None" << std::endl; + std::cout << std::endl; + // clang-format on +} + +int main(int argc, char* argv[]) +{ + // Program Setup + // ============================================================================================= + // Parse parameters + if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { + PrintHelp(); + return 1; + } + // Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456 + std::string robot_sn = argv[1]; + + // Print description + spdlog::info( + ">>> Tutorial description <<<\nThis tutorial shows how to change the logging behaviors of " + "RDK client."); + + // Suppress log messages from RDK client + // ========================================================================================= + // Change log level to suppress certain type of log messages from RDK client. The log level + // setting will apply globally to all following spdlog messages + spdlog::set_level(spdlog::level::warn); ///< Suppress info messages + // messages spdlog::set_level(spdlog::level::off); ///< Suppress all messages + + // Instantiate RDK client, all info messages are suppressed + try { + flexiv::rdk::Robot robot(robot_sn); + } catch (const std::exception& e) { + spdlog::error(e.what()); + } + + // Output all log messages to a file + // ========================================================================================= + // Create a logger that outputs the messages to both the console and a log file + auto console_sink = std::make_shared(); + auto file_sink = std::make_shared("example.log", true); + std::vector sinks {console_sink, file_sink}; + // Since writing to file takes some time, so we use async logger to avoid blocking the program + spdlog::init_thread_pool(10240, 1); + auto tp = spdlog::thread_pool(); + auto logger + = std::make_shared("global_logger", sinks.begin(), sinks.end(), tp); + // Use default pattern for log messages + logger->set_pattern(kDefaultLogPattern); + + // Use the logger configured above as the default global logger + spdlog::set_default_logger(logger); + + // Allow all messages + spdlog::set_level(spdlog::level::info); + + // Instantiate RDK client again, all messages are printed to console and output to a log file + try { + flexiv::rdk::Robot robot(robot_sn); + } catch (const std::exception& e) { + spdlog::error(e.what()); + } + + spdlog::warn("This message should also appear in the log file"); + spdlog::info("Program finished"); + + return 0; +} diff --git a/example/intermediate1_realtime_joint_position_control.cpp b/example/intermediate1_realtime_joint_position_control.cpp index ebcdf855..7b0afb32 100644 --- a/example/intermediate1_realtime_joint_position_control.cpp +++ b/example/intermediate1_realtime_joint_position_control.cpp @@ -1,14 +1,14 @@ /** * @example intermediate1_realtime_joint_position_control.cpp * This tutorial runs real-time joint position control to hold or sine-sweep all robot joints. - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -28,15 +28,6 @@ constexpr double kSineFreq = 0.3; std::atomic g_stop_sched = {false}; } -/** @brief Print tutorial description */ -void PrintDescription() -{ - std::cout << "This tutorial runs real-time joint position control to hold or sine-sweep all " - "robot joints." - << std::endl - << std::endl; -} - /** @brief Print program usage help */ void PrintHelp() { @@ -51,8 +42,8 @@ void PrintHelp() } /** @brief Callback function for realtime periodic task */ -void PeriodicTask(flexiv::Robot& robot, flexiv::Log& log, const std::string& motion_type, - const std::array& init_pos) +void PeriodicTask( + flexiv::rdk::Robot& robot, const std::string& motion_type, const std::vector& init_pos) { // Local periodic loop counter static unsigned int loop_counter = 0; @@ -64,16 +55,16 @@ void PeriodicTask(flexiv::Robot& robot, flexiv::Log& log, const std::string& mot "PeriodicTask: Fault occurred on the connected robot, exiting ..."); } - // Initialize target arrays to hold position - std::array target_pos = {}; - std::array target_vel = {}; - std::array target_acc = {}; + // Initialize target vectors to hold position + std::vector target_pos(robot.info().DoF); + std::vector target_vel(robot.info().DoF); + std::vector target_acc(robot.info().DoF); - // Set target arrays based on motion type + // Set target vectors based on motion type if (motion_type == "hold") { target_pos = init_pos; } else if (motion_type == "sine-sweep") { - for (size_t i = 0; i < flexiv::kJointDOF; ++i) { + for (size_t i = 0; i < target_pos.size(); ++i) { target_pos[i] = init_pos[i] + kSineAmp * sin(2 * M_PI * kSineFreq * loop_counter * kLoopPeriod); } @@ -88,7 +79,7 @@ void PeriodicTask(flexiv::Robot& robot, flexiv::Log& log, const std::string& mot loop_counter++; } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); g_stop_sched = true; } } @@ -97,11 +88,8 @@ int main(int argc, char* argv[]) { // Program Setup // ============================================================================================= - // Logger for printing message with timestamp and coloring - flexiv::Log log; - // Parse parameters - if (argc < 2 || flexiv::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { + if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { PrintHelp(); return 1; } @@ -109,16 +97,17 @@ int main(int argc, char* argv[]) std::string robot_sn = argv[1]; // Print description - log.Info("Tutorial description:"); - PrintDescription(); + spdlog::info( + ">>> Tutorial description <<<\nThis tutorial runs real-time joint position control to hold " + "or sine-sweep all robot joints."); // Type of motion specified by user std::string motion_type = ""; - if (flexiv::utility::ProgramArgsExist(argc, argv, "--hold")) { - log.Info("Robot holding current pose"); + if (flexiv::rdk::utility::ProgramArgsExist(argc, argv, "--hold")) { + spdlog::info("Robot holding current pose"); motion_type = "hold"; } else { - log.Info("Robot running joint sine-sweep"); + spdlog::info("Robot running joint sine-sweep"); motion_type = "sine-sweep"; } @@ -126,32 +115,32 @@ int main(int argc, char* argv[]) // RDK Initialization // ========================================================================================= // Instantiate robot interface - flexiv::Robot robot(robot_sn); + flexiv::rdk::Robot robot(robot_sn); // Clear fault on the connected robot if any if (robot.fault()) { - log.Warn("Fault occurred on the connected robot, trying to clear ..."); + spdlog::warn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault if (!robot.ClearFault()) { - log.Error("Fault cannot be cleared, exiting ..."); + spdlog::error("Fault cannot be cleared, exiting ..."); return 1; } - log.Info("Fault on the connected robot is cleared"); + spdlog::info("Fault on the connected robot is cleared"); } // Enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ..."); + spdlog::info("Enabling robot ..."); robot.Enable(); // Wait for the robot to become operational while (!robot.operational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); } - log.Info("Robot is now operational"); + spdlog::info("Robot is now operational"); // Move robot to home pose - log.Info("Moving to home pose"); - robot.SwitchMode(flexiv::Mode::NRT_PRIMITIVE_EXECUTION); + spdlog::info("Moving to home pose"); + robot.SwitchMode(flexiv::rdk::Mode::NRT_PRIMITIVE_EXECUTION); robot.ExecutePrimitive("Home()"); // Wait for the primitive to finish @@ -162,17 +151,17 @@ int main(int argc, char* argv[]) // Real-time Joint Position Control // ========================================================================================= // Switch to real-time joint position control mode - robot.SwitchMode(flexiv::Mode::RT_JOINT_POSITION); + robot.SwitchMode(flexiv::rdk::Mode::RT_JOINT_POSITION); // Set initial joint positions auto init_pos = robot.states().q; - log.Info("Initial joint positions set to: " + flexiv::utility::Arr2Str(init_pos)); + spdlog::info("Initial joint positions set to: {}", flexiv::rdk::utility::Vec2Str(init_pos)); // Create real-time scheduler to run periodic tasks - flexiv::Scheduler scheduler; + flexiv::rdk::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority - scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::ref(log), - std::ref(motion_type), std::ref(init_pos)), + scheduler.AddTask( + std::bind(PeriodicTask, std::ref(robot), std::ref(motion_type), std::ref(init_pos)), "HP periodic", 1, scheduler.max_priority()); // Start all added tasks scheduler.Start(); @@ -185,7 +174,7 @@ int main(int argc, char* argv[]) scheduler.Stop(); } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); return 1; } diff --git a/example/intermediate2_realtime_joint_impedance_control.cpp b/example/intermediate2_realtime_joint_impedance_control.cpp new file mode 100644 index 00000000..02884642 --- /dev/null +++ b/example/intermediate2_realtime_joint_impedance_control.cpp @@ -0,0 +1,201 @@ +/** + * @example intermediate2_realtime_joint_impedance_control.cpp + * This tutorial runs real-time joint impedance control to hold or sine-sweep all robot joints. + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. + * @author Flexiv + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace { +/** RT loop period [sec] */ +constexpr double kLoopPeriod = 0.001; + +/** Sine-sweep trajectory amplitude and frequency */ +constexpr double kSineAmp = 0.035; +constexpr double kSineFreq = 0.3; + +/** Atomic signal to stop scheduler tasks */ +std::atomic g_stop_sched = {false}; +} + +/** @brief Print program usage help */ +void PrintHelp() +{ + // clang-format off + std::cout << "Required arguments: [robot SN]" << std::endl; + std::cout << " robot SN: Serial number of the robot to connect to. " + "Remove any space, for example: Rizon4s-123456" << std::endl; + std::cout << "Optional arguments: [--hold]" << std::endl; + std::cout << " --hold: robot holds current joint positions, otherwise do a sine-sweep" << std::endl; + std::cout << std::endl; + // clang-format on +} + +/** @brief Callback function for realtime periodic task */ +void PeriodicTask( + flexiv::rdk::Robot& robot, const std::string& motion_type, const std::vector& init_pos) +{ + // Local periodic loop counter + static unsigned int loop_counter = 0; + + try { + // Monitor fault on the connected robot + if (robot.fault()) { + throw std::runtime_error( + "PeriodicTask: Fault occurred on the connected robot, exiting ..."); + } + + // Initialize target vectors to hold position + std::vector target_pos(robot.info().DoF); + std::vector target_vel(robot.info().DoF); + std::vector target_acc(robot.info().DoF); + + // Set target vectors based on motion type + if (motion_type == "hold") { + target_pos = init_pos; + } else if (motion_type == "sine-sweep") { + for (size_t i = 0; i < target_pos.size(); ++i) { + target_pos[i] = init_pos[i] + + kSineAmp * sin(2 * M_PI * kSineFreq * loop_counter * kLoopPeriod); + } + } else { + throw std::invalid_argument( + "PeriodicTask: unknown motion type. Accepted motion types: hold, sine-sweep"); + } + + // Reduce joint stiffness to half of nominal values after 5 seconds + if (loop_counter == 5000) { + auto new_Kq = robot.info().K_q_nom; + for (auto& v : new_Kq) { + v *= 0.5; + } + robot.SetJointImpedance(new_Kq); + spdlog::info( + "PeriodicTask: joint stiffness set to [{}]", flexiv::rdk::utility::Vec2Str(new_Kq)); + } + + // Reset joint stiffness to nominal values after another 5 seconds + if (loop_counter == 10000) { + robot.ResetJointImpedance(); + spdlog::info("PeriodicTask: joint stiffness reset to [{}]", + flexiv::rdk::utility::Vec2Str(robot.info().K_q_nom)); + } + + // Send commands + robot.StreamJointPosition(target_pos, target_vel, target_acc); + + // Increment loop counter + loop_counter++; + + } catch (const std::exception& e) { + spdlog::error(e.what()); + g_stop_sched = true; + } +} + +int main(int argc, char* argv[]) +{ + // Program Setup + // ============================================================================================= + // Parse parameters + if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { + PrintHelp(); + return 1; + } + // Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456 + std::string robot_sn = argv[1]; + + // Print description + spdlog::info( + ">>> Tutorial description <<<\nThis tutorial runs real-time joint impedance control to " + "hold or sine-sweep all robot joints."); + + // Type of motion specified by user + std::string motion_type = ""; + if (flexiv::rdk::utility::ProgramArgsExist(argc, argv, "--hold")) { + spdlog::info("Robot holding current pose"); + motion_type = "hold"; + } else { + spdlog::info("Robot running joint sine-sweep"); + motion_type = "sine-sweep"; + } + + try { + // RDK Initialization + // ========================================================================================= + // Instantiate robot interface + flexiv::rdk::Robot robot(robot_sn); + + // Clear fault on the connected robot if any + if (robot.fault()) { + spdlog::warn("Fault occurred on the connected robot, trying to clear ..."); + // Try to clear the fault + if (!robot.ClearFault()) { + spdlog::error("Fault cannot be cleared, exiting ..."); + return 1; + } + spdlog::info("Fault on the connected robot is cleared"); + } + + // Enable the robot, make sure the E-stop is released before enabling + spdlog::info("Enabling robot ..."); + robot.Enable(); + + // Wait for the robot to become operational + while (!robot.operational()) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + spdlog::info("Robot is now operational"); + + // Move robot to home pose + spdlog::info("Moving to home pose"); + robot.SwitchMode(flexiv::rdk::Mode::NRT_PRIMITIVE_EXECUTION); + robot.ExecutePrimitive("Home()"); + + // Wait for the primitive to finish + while (robot.busy()) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + // Real-time Joint Impedance Control + // ========================================================================================= + // Switch to real-time joint impedance control mode + robot.SwitchMode(flexiv::rdk::Mode::RT_JOINT_IMPEDANCE); + + // Set initial joint positions + auto init_pos = robot.states().q; + spdlog::info("Initial joint positions set to: {}", flexiv::rdk::utility::Vec2Str(init_pos)); + + // Create real-time scheduler to run periodic tasks + flexiv::rdk::Scheduler scheduler; + // Add periodic task with 1ms interval and highest applicable priority + scheduler.AddTask( + std::bind(PeriodicTask, std::ref(robot), std::ref(motion_type), std::ref(init_pos)), + "HP periodic", 1, scheduler.max_priority()); + // Start all added tasks + scheduler.Start(); + + // Block and wait for signal to stop scheduler tasks + while (!g_stop_sched) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + // Received signal to stop scheduler tasks + scheduler.Stop(); + + } catch (const std::exception& e) { + spdlog::error(e.what()); + return 1; + } + + return 0; +} diff --git a/example/intermediate2_realtime_joint_torque_control.cpp b/example/intermediate3_realtime_joint_torque_control.cpp similarity index 65% rename from example/intermediate2_realtime_joint_torque_control.cpp rename to example/intermediate3_realtime_joint_torque_control.cpp index fb4b51e1..23a76325 100644 --- a/example/intermediate2_realtime_joint_torque_control.cpp +++ b/example/intermediate3_realtime_joint_torque_control.cpp @@ -1,16 +1,19 @@ /** - * @example intermediate2_realtime_joint_torque_control.cpp + * @example intermediate3_realtime_joint_torque_control.cpp * This tutorial runs real-time joint torque control to hold or sine-sweep all robot joints. An * outer position loop is used to generate joint torque commands. This outer position loop + inner * torque loop together is also known as an impedance controller. - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * @warning The impedance controller in this example is for demo purpose only and has no performance + * guarantee. To use the robot's built-in high-performance joint impedance controller, please refer + * to intermediate2_realtime_joint_impedance_control.cpp. + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -23,9 +26,8 @@ namespace { constexpr double kLoopPeriod = 0.001; /** Outer position loop (impedance) gains, values are only for demo purpose */ -const std::array kImpedanceKp - = {3000.0, 3000.0, 800.0, 800.0, 200.0, 200.0, 200.0}; -const std::array kImpedanceKd = {80.0, 80.0, 40.0, 40.0, 8.0, 8.0, 8.0}; +const std::vector kImpedanceKp = {3000.0, 3000.0, 800.0, 800.0, 200.0, 200.0, 200.0}; +const std::vector kImpedanceKd = {80.0, 80.0, 40.0, 40.0, 8.0, 8.0, 8.0}; /** Sine-sweep trajectory amplitude and frequency */ constexpr double kSineAmp = 0.035; @@ -35,17 +37,6 @@ constexpr double kSineFreq = 0.3; std::atomic g_stop_sched = {false}; } -/** @brief Print tutorial description */ -void PrintDescription() -{ - std::cout - << "This tutorial runs real-time joint torque control to hold or sine-sweep all robot " - "joints. An outer position loop is used to generate joint torque commands. This outer " - "position loop + inner torque loop together is also known as an impedance controller." - << std::endl - << std::endl; -} - /** @brief Print program usage help */ void PrintHelp() { @@ -60,8 +51,8 @@ void PrintHelp() } /** @brief Callback function for realtime periodic task */ -void PeriodicTask(flexiv::Robot& robot, flexiv::Log& log, const std::string& motion_type, - const std::array& init_pos) +void PeriodicTask( + flexiv::rdk::Robot& robot, const std::string& motion_type, const std::vector& init_pos) { // Local periodic loop counter static unsigned int loop_counter = 0; @@ -74,13 +65,13 @@ void PeriodicTask(flexiv::Robot& robot, flexiv::Log& log, const std::string& mot } // Target joint positions - std::array target_pos = {}; + std::vector target_pos(robot.info().DoF); // Set target position based on motion type if (motion_type == "hold") { target_pos = init_pos; } else if (motion_type == "sine-sweep") { - for (size_t i = 0; i < flexiv::kJointDOF; ++i) { + for (size_t i = 0; i < target_pos.size(); ++i) { target_pos[i] = init_pos[i] + kSineAmp * sin(2 * M_PI * kSineFreq * loop_counter * kLoopPeriod); } @@ -90,8 +81,8 @@ void PeriodicTask(flexiv::Robot& robot, flexiv::Log& log, const std::string& mot } // Run impedance control on all joints - std::array target_torque = {}; - for (size_t i = 0; i < flexiv::kJointDOF; ++i) { + std::vector target_torque(robot.info().DoF); + for (size_t i = 0; i < target_torque.size(); ++i) { target_torque[i] = kImpedanceKp[i] * (target_pos[i] - robot.states().q[i]) - kImpedanceKd[i] * robot.states().dtheta[i]; } @@ -102,7 +93,7 @@ void PeriodicTask(flexiv::Robot& robot, flexiv::Log& log, const std::string& mot loop_counter++; } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); g_stop_sched = true; } } @@ -111,11 +102,8 @@ int main(int argc, char* argv[]) { // Program Setup // ============================================================================================= - // Logger for printing message with timestamp and coloring - flexiv::Log log; - // Parse parameters - if (argc < 2 || flexiv::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { + if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { PrintHelp(); return 1; } @@ -123,16 +111,19 @@ int main(int argc, char* argv[]) std::string robot_sn = argv[1]; // Print description - log.Info("Tutorial description:"); - PrintDescription(); + spdlog::info( + ">>> Tutorial description <<<\nThis tutorial runs real-time joint torque control to hold " + "or sine-sweep all robot joints. An outer position loop is used to generate joint torque " + "commands. This outer position loop + inner torque loop together is also known as an " + "impedance controller."); // Type of motion specified by user std::string motion_type = ""; - if (flexiv::utility::ProgramArgsExist(argc, argv, "--hold")) { - log.Info("Robot holding current pose"); + if (flexiv::rdk::utility::ProgramArgsExist(argc, argv, "--hold")) { + spdlog::info("Robot holding current pose"); motion_type = "hold"; } else { - log.Info("Robot running joint sine-sweep"); + spdlog::info("Robot running joint sine-sweep"); motion_type = "sine-sweep"; } @@ -140,32 +131,32 @@ int main(int argc, char* argv[]) // RDK Initialization // ========================================================================================= // Instantiate robot interface - flexiv::Robot robot(robot_sn); + flexiv::rdk::Robot robot(robot_sn); // Clear fault on the connected robot if any if (robot.fault()) { - log.Warn("Fault occurred on the connected robot, trying to clear ..."); + spdlog::warn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault if (!robot.ClearFault()) { - log.Error("Fault cannot be cleared, exiting ..."); + spdlog::error("Fault cannot be cleared, exiting ..."); return 1; } - log.Info("Fault on the connected robot is cleared"); + spdlog::info("Fault on the connected robot is cleared"); } // Enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ..."); + spdlog::info("Enabling robot ..."); robot.Enable(); // Wait for the robot to become operational while (!robot.operational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); } - log.Info("Robot is now operational"); + spdlog::info("Robot is now operational"); // Move robot to home pose - log.Info("Moving to home pose"); - robot.SwitchMode(flexiv::Mode::NRT_PRIMITIVE_EXECUTION); + spdlog::info("Moving to home pose"); + robot.SwitchMode(flexiv::rdk::Mode::NRT_PRIMITIVE_EXECUTION); robot.ExecutePrimitive("Home()"); // Wait for the primitive to finish @@ -176,17 +167,17 @@ int main(int argc, char* argv[]) // Real-time Joint Torque Control // ========================================================================================= // Switch to real-time joint torque control mode - robot.SwitchMode(flexiv::Mode::RT_JOINT_TORQUE); + robot.SwitchMode(flexiv::rdk::Mode::RT_JOINT_TORQUE); // Set initial joint positions auto init_pos = robot.states().q; - log.Info("Initial joint positions set to: " + flexiv::utility::Arr2Str(init_pos)); + spdlog::info("Initial joint positions set to: {}", flexiv::rdk::utility::Vec2Str(init_pos)); // Create real-time scheduler to run periodic tasks - flexiv::Scheduler scheduler; + flexiv::rdk::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority - scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::ref(log), - std::ref(motion_type), std::ref(init_pos)), + scheduler.AddTask( + std::bind(PeriodicTask, std::ref(robot), std::ref(motion_type), std::ref(init_pos)), "HP periodic", 1, scheduler.max_priority()); // Start all added tasks scheduler.Start(); @@ -199,7 +190,7 @@ int main(int argc, char* argv[]) scheduler.Stop(); } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); return 1; } diff --git a/example/intermediate3_realtime_joint_floating.cpp b/example/intermediate4_realtime_joint_floating.cpp similarity index 64% rename from example/intermediate3_realtime_joint_floating.cpp rename to example/intermediate4_realtime_joint_floating.cpp index 11d44cd3..e0810be3 100644 --- a/example/intermediate3_realtime_joint_floating.cpp +++ b/example/intermediate4_realtime_joint_floating.cpp @@ -1,17 +1,17 @@ /** - * @example intermediate3_realtime_joint_floating.cpp + * @example intermediate4_realtime_joint_floating.cpp * This tutorial runs real-time joint floating with gentle velocity damping, gravity compensation, * and soft protection against position limits. This example is ideal for verifying the system's * whole-loop real-timeliness, accuracy of the robot dynamics model, and joint torque control * performance. If everything works well, all joints should float smoothly. - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -20,25 +20,12 @@ namespace { /** Joint velocity damping gains for floating */ -const std::array kFloatingDamping - = {10.0, 10.0, 5.0, 5.0, 1.0, 1.0, 1.0}; +const std::vector kFloatingDamping = {10.0, 10.0, 5.0, 5.0, 1.0, 1.0, 1.0}; /** Atomic signal to stop scheduler tasks */ std::atomic g_stop_sched = {false}; } -/** @brief Print tutorial description */ -void PrintDescription() -{ - std::cout << "This tutorial runs real-time joint floating with gentle velocity damping, " - "gravity compensation, and soft protection against position limits. This example " - "is ideal for verifying the system's whole-loop real-timeliness, accuracy of the " - "robot dynamics model, and joint torque control performance. If everything works " - "well, all joints should float smoothly." - << std::endl - << std::endl; -} - /** @brief Print program usage help */ void PrintHelp() { @@ -52,7 +39,7 @@ void PrintHelp() } /** @brief Callback function for realtime periodic task */ -void PeriodicTask(flexiv::Robot& robot, flexiv::Log& log) +void PeriodicTask(flexiv::rdk::Robot& robot) { try { // Monitor fault on the connected robot @@ -62,10 +49,10 @@ void PeriodicTask(flexiv::Robot& robot, flexiv::Log& log) } // Set 0 joint torques - std::array target_torque = {}; + std::vector target_torque(robot.info().DoF); // Add some velocity damping - for (size_t i = 0; i < flexiv::kJointDOF; ++i) { + for (size_t i = 0; i < target_torque.size(); ++i) { target_torque[i] += -kFloatingDamping[i] * robot.states().dtheta[i]; } @@ -74,7 +61,7 @@ void PeriodicTask(flexiv::Robot& robot, flexiv::Log& log) robot.StreamJointTorque(target_torque, true, true); } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); g_stop_sched = true; } } @@ -83,11 +70,8 @@ int main(int argc, char* argv[]) { // Program Setup // ============================================================================================= - // Logger for printing message with timestamp and coloring - flexiv::Log log; - // Parse parameters - if (argc < 2 || flexiv::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { + if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { PrintHelp(); return 1; } @@ -95,39 +79,43 @@ int main(int argc, char* argv[]) std::string robot_sn = argv[1]; // Print description - log.Info("Tutorial description:"); - PrintDescription(); + spdlog::info( + ">>> Tutorial description <<<\nThis tutorial runs real-time joint floating with gentle " + "velocity damping, gravity compensation, and soft protection against position limits. This " + "example is ideal for verifying the system's whole-loop real-timeliness, accuracy of the " + "robot dynamics model, and joint torque control performance. If everything works well, all " + "joints should float smoothly."); try { // RDK Initialization // ========================================================================================= // Instantiate robot interface - flexiv::Robot robot(robot_sn); + flexiv::rdk::Robot robot(robot_sn); // Clear fault on the connected robot if any if (robot.fault()) { - log.Warn("Fault occurred on the connected robot, trying to clear ..."); + spdlog::warn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault if (!robot.ClearFault()) { - log.Error("Fault cannot be cleared, exiting ..."); + spdlog::error("Fault cannot be cleared, exiting ..."); return 1; } - log.Info("Fault on the connected robot is cleared"); + spdlog::info("Fault on the connected robot is cleared"); } // Enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ..."); + spdlog::info("Enabling robot ..."); robot.Enable(); // Wait for the robot to become operational while (!robot.operational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); } - log.Info("Robot is now operational"); + spdlog::info("Robot is now operational"); // Move robot to home pose - log.Info("Moving to home pose"); - robot.SwitchMode(flexiv::Mode::NRT_PRIMITIVE_EXECUTION); + spdlog::info("Moving to home pose"); + robot.SwitchMode(flexiv::rdk::Mode::NRT_PRIMITIVE_EXECUTION); robot.ExecutePrimitive("Home()"); // Wait for the primitive to finish @@ -138,13 +126,13 @@ int main(int argc, char* argv[]) // Real-time Joint Floating // ========================================================================================= // Switch to real-time joint torque control mode - robot.SwitchMode(flexiv::Mode::RT_JOINT_TORQUE); + robot.SwitchMode(flexiv::rdk::Mode::RT_JOINT_TORQUE); // Create real-time scheduler to run periodic tasks - flexiv::Scheduler scheduler; + flexiv::rdk::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority - scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::ref(log)), "HP periodic", 1, - scheduler.max_priority()); + scheduler.AddTask( + std::bind(PeriodicTask, std::ref(robot)), "HP periodic", 1, scheduler.max_priority()); // Start all added tasks scheduler.Start(); @@ -156,7 +144,7 @@ int main(int argc, char* argv[]) scheduler.Stop(); } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); return 1; } diff --git a/example/intermediate4_realtime_cartesian_pure_motion_control.cpp b/example/intermediate5_realtime_cartesian_pure_motion_control.cpp similarity index 71% rename from example/intermediate4_realtime_cartesian_pure_motion_control.cpp rename to example/intermediate5_realtime_cartesian_pure_motion_control.cpp index 61425cff..d036d379 100644 --- a/example/intermediate4_realtime_cartesian_pure_motion_control.cpp +++ b/example/intermediate5_realtime_cartesian_pure_motion_control.cpp @@ -1,15 +1,15 @@ /** - * @example intermediate4_realtime_cartesian_pure_motion_control.cpp + * @example intermediate5_realtime_cartesian_pure_motion_control.cpp * This tutorial runs real-time Cartesian-space pure motion control to hold or sine-sweep the robot * TCP. A simple collision detection is also included. - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -39,15 +39,6 @@ constexpr double kExtTorqueThreshold = 5.0; std::atomic g_stop_sched = {false}; } -/** @brief Print tutorial description */ -void PrintDescription() -{ - std::cout << "This tutorial runs real-time Cartesian-space pure motion control to hold or " - "sine-sweep the robot TCP. A simple collision detection is also included." - << std::endl - << std::endl; -} - /** @brief Print program usage help */ void PrintHelp() { @@ -63,8 +54,9 @@ void PrintHelp() } /** @brief Callback function for realtime periodic task */ -void PeriodicTask(flexiv::Robot& robot, flexiv::Log& log, - const std::array& init_pose, bool enable_hold, bool enable_collision) +void PeriodicTask(flexiv::rdk::Robot& robot, + const std::array& init_pose, bool enable_hold, + bool enable_collision) { // Local periodic loop counter static uint64_t loop_counter = 0; @@ -94,49 +86,52 @@ void PeriodicTask(flexiv::Robot& robot, flexiv::Log& log, switch (loop_counter % (20 * kLoopFreq)) { // Online change preferred joint positions at 3 seconds case (3 * kLoopFreq): { - std::array preferred_jnt_pos + std::vector preferred_jnt_pos = {0.938, -1.108, -1.254, 1.464, 1.073, 0.278, -0.658}; robot.SetNullSpacePosture(preferred_jnt_pos); - log.Info("Preferred joint positions set to: " - + flexiv::utility::Arr2Str(preferred_jnt_pos)); + spdlog::info("Preferred joint positions set to: " + + flexiv::rdk::utility::Vec2Str(preferred_jnt_pos)); } break; // Online change stiffness to half of nominal at 6 seconds case (6 * kLoopFreq): { - auto new_K = robot.info().nominal_K; + auto new_K = robot.info().K_x_nom; for (auto& v : new_K) { v *= 0.5; } - robot.SetCartesianStiffness(new_K); - log.Info("Cartesian stiffness set to: " + flexiv::utility::Arr2Str(new_K)); + robot.SetCartesianImpedance(new_K); + spdlog::info( + "Cartesian stiffness set to: {}", flexiv::rdk::utility::Arr2Str(new_K)); } break; // Online change to another preferred joint positions at 9 seconds case (9 * kLoopFreq): { - std::array preferred_jnt_pos + std::vector preferred_jnt_pos = {-0.938, -1.108, 1.254, 1.464, -1.073, 0.278, 0.658}; robot.SetNullSpacePosture(preferred_jnt_pos); - log.Info("Preferred joint positions set to: " - + flexiv::utility::Arr2Str(preferred_jnt_pos)); + spdlog::info("Preferred joint positions set to: " + + flexiv::rdk::utility::Vec2Str(preferred_jnt_pos)); } break; // Online reset stiffness to nominal at 12 seconds case (12 * kLoopFreq): { - robot.ResetCartesianStiffness(); - log.Info("Cartesian stiffness is reset"); + robot.ResetCartesianImpedance(); + spdlog::info("Cartesian stiffness is reset"); } break; // Online reset preferred joint positions to nominal at 14 seconds case (14 * kLoopFreq): { robot.ResetNullSpacePosture(); - log.Info("Preferred joint positions are reset"); + spdlog::info("Preferred joint positions are reset"); } break; // Online enable max contact wrench regulation at 16 seconds case (16 * kLoopFreq): { - std::array max_wrench = {10.0, 10.0, 10.0, 2.0, 2.0, 2.0}; + std::array max_wrench + = {10.0, 10.0, 10.0, 2.0, 2.0, 2.0}; robot.SetMaxContactWrench(max_wrench); - log.Info("Max contact wrench set to: " + flexiv::utility::Arr2Str(max_wrench)); + spdlog::info( + "Max contact wrench set to: {}", flexiv::rdk::utility::Arr2Str(max_wrench)); } break; // Disable max contact wrench regulation at 19 seconds case (19 * kLoopFreq): { robot.ResetMaxContactWrench(); - log.Info("Max contact wrench is reset"); + spdlog::info("Max contact wrench is reset"); } break; default: break; @@ -158,7 +153,7 @@ void PeriodicTask(flexiv::Robot& robot, flexiv::Log& log, } if (collision_detected) { robot.Stop(); - log.Warn("Collision detected, stopping robot and exit program ..."); + spdlog::warn("Collision detected, stopping robot and exit program ..."); g_stop_sched = true; } } @@ -167,7 +162,7 @@ void PeriodicTask(flexiv::Robot& robot, flexiv::Log& log, loop_counter++; } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); g_stop_sched = true; } } @@ -176,11 +171,8 @@ int main(int argc, char* argv[]) { // Program Setup // ============================================================================================= - // Logger for printing message with timestamp and coloring - flexiv::Log log; - // Parse parameters - if (argc < 2 || flexiv::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { + if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { PrintHelp(); return 1; } @@ -188,57 +180,59 @@ int main(int argc, char* argv[]) std::string robot_sn = argv[1]; // Print description - log.Info("Tutorial description:"); - PrintDescription(); + spdlog::info( + ">>> Tutorial description <<<\nThis tutorial runs real-time Cartesian-space pure motion " + "control to hold or sine-sweep the robot TCP. A simple collision detection is also " + "included."); // Type of motion specified by user bool enable_hold = false; - if (flexiv::utility::ProgramArgsExist(argc, argv, "--hold")) { - log.Info("Robot holding current TCP pose"); + if (flexiv::rdk::utility::ProgramArgsExist(argc, argv, "--hold")) { + spdlog::info("Robot holding current TCP pose"); enable_hold = true; } else { - log.Info("Robot running TCP sine-sweep"); + spdlog::info("Robot running TCP sine-sweep"); } // Whether to enable collision detection bool enable_collision = false; - if (flexiv::utility::ProgramArgsExist(argc, argv, "--collision")) { - log.Info("Collision detection enabled"); + if (flexiv::rdk::utility::ProgramArgsExist(argc, argv, "--collision")) { + spdlog::info("Collision detection enabled"); enable_collision = true; } else { - log.Info("Collision detection disabled"); + spdlog::info("Collision detection disabled"); } try { // RDK Initialization // ========================================================================================= // Instantiate robot interface - flexiv::Robot robot(robot_sn); + flexiv::rdk::Robot robot(robot_sn); // Clear fault on the connected robot if any if (robot.fault()) { - log.Warn("Fault occurred on the connected robot, trying to clear ..."); + spdlog::warn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault if (!robot.ClearFault()) { - log.Error("Fault cannot be cleared, exiting ..."); + spdlog::error("Fault cannot be cleared, exiting ..."); return 1; } - log.Info("Fault on the connected robot is cleared"); + spdlog::info("Fault on the connected robot is cleared"); } // Enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ..."); + spdlog::info("Enabling robot ..."); robot.Enable(); // Wait for the robot to become operational while (!robot.operational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); } - log.Info("Robot is now operational"); + spdlog::info("Robot is now operational"); // Move robot to home pose - log.Info("Moving to home pose"); - robot.SwitchMode(flexiv::Mode::NRT_PRIMITIVE_EXECUTION); + spdlog::info("Moving to home pose"); + robot.SwitchMode(flexiv::rdk::Mode::NRT_PRIMITIVE_EXECUTION); robot.ExecutePrimitive("Home()"); // Wait for the primitive to finish @@ -253,13 +247,14 @@ int main(int argc, char* argv[]) // WARNING: during the process, the robot must not contact anything, otherwise the result // will be inaccurate and affect following operations - log.Warn("Zeroing force/torque sensors, make sure nothing is in contact with the robot"); + spdlog::warn( + "Zeroing force/torque sensors, make sure nothing is in contact with the robot"); // Wait for primitive completion while (robot.busy()) { std::this_thread::sleep_for(std::chrono::seconds(1)); } - log.Info("Sensor zeroing complete"); + spdlog::info("Sensor zeroing complete"); // Configure Motion Control // ========================================================================================= @@ -272,18 +267,18 @@ int main(int argc, char* argv[]) // Start Pure Motion Control // ========================================================================================= // Switch to real-time mode for continuous motion control - robot.SwitchMode(flexiv::Mode::RT_CARTESIAN_MOTION_FORCE); + robot.SwitchMode(flexiv::rdk::Mode::RT_CARTESIAN_MOTION_FORCE); // Set initial pose to current TCP pose auto init_pose = robot.states().tcp_pose; - log.Info("Initial TCP pose set to [position 3x1, rotation (quaternion) 4x1]: " - + flexiv::utility::Arr2Str(init_pose)); + spdlog::info("Initial TCP pose set to [position 3x1, rotation (quaternion) 4x1]: " + + flexiv::rdk::utility::Arr2Str(init_pose)); // Create real-time scheduler to run periodic tasks - flexiv::Scheduler scheduler; + flexiv::rdk::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority - scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::ref(log), - std::ref(init_pose), enable_hold, enable_collision), + scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::ref(init_pose), enable_hold, + enable_collision), "HP periodic", 1, scheduler.max_priority()); // Start all added tasks scheduler.Start(); @@ -296,7 +291,7 @@ int main(int argc, char* argv[]) scheduler.Stop(); } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); return 1; } diff --git a/example/intermediate5_realtime_cartesian_motion_force_control.cpp b/example/intermediate6_realtime_cartesian_motion_force_control.cpp similarity index 77% rename from example/intermediate5_realtime_cartesian_motion_force_control.cpp rename to example/intermediate6_realtime_cartesian_motion_force_control.cpp index 7909d8bc..82d6d518 100644 --- a/example/intermediate5_realtime_cartesian_motion_force_control.cpp +++ b/example/intermediate6_realtime_cartesian_motion_force_control.cpp @@ -1,16 +1,16 @@ /** - * @example intermediate5_realtime_cartesian_motion_force_control.cpp + * @example intermediate6_realtime_cartesian_motion_force_control.cpp * This tutorial runs real-time Cartesian-space unified motion-force control. The Z axis of the * chosen reference frame will be activated for explicit force control, while the rest axes in the * same reference frame will stay motion controlled. - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -37,23 +37,13 @@ constexpr double kSearchVelocity = 0.02; constexpr double kSearchDistance = 1.0; /** Maximum contact wrench during contact search for soft contact */ -const std::array kMaxWrenchForContactSearch +const std::array kMaxWrenchForContactSearch = {10.0, 10.0, 10.0, 3.0, 3.0, 3.0}; /** Atomic signal to stop scheduler tasks */ std::atomic g_stop_sched = {false}; } -/** @brief Print tutorial description */ -void PrintDescription() -{ - std::cout << "This tutorial runs real-time Cartesian-space unified motion-force control. The Z " - "axis of the chosen reference frame will be activated for explicit force control, " - "while the rest axes in the same reference frame will stay motion controlled." - << std::endl - << std::endl; -} - /** @brief Print program usage help */ void PrintHelp() { @@ -70,8 +60,8 @@ void PrintHelp() } /** Callback function for realtime periodic task */ -void PeriodicTask(flexiv::Robot& robot, flexiv::Log& log, - const std::array& init_pose, const std::string frame_str, +void PeriodicTask(flexiv::rdk::Robot& robot, + const std::array& init_pose, const std::string frame_str, bool enable_polish) { // Local periodic loop counter @@ -94,7 +84,7 @@ void PeriodicTask(flexiv::Robot& robot, flexiv::Log& log, } else if (frame_str == "TCP") { Fz = kPressingForce; } - std::array target_wrench = {0.0, 0.0, Fz, 0.0, 0.0, 0.0}; + std::array target_wrench = {0.0, 0.0, Fz, 0.0, 0.0, 0.0}; // Apply constant force along Z axis of chosen reference frame, and do a simple polish // motion along XY plane in robot world frame @@ -116,7 +106,7 @@ void PeriodicTask(flexiv::Robot& robot, flexiv::Log& log, loop_counter++; } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); g_stop_sched = true; } } @@ -125,11 +115,8 @@ int main(int argc, char* argv[]) { // Program Setup // ============================================================================================= - // Logger for printing message with timestamp and coloring - flexiv::Log log; - // Parse parameters - if (argc < 2 || flexiv::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { + if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { PrintHelp(); return 1; } @@ -137,58 +124,61 @@ int main(int argc, char* argv[]) std::string robot_sn = argv[1]; // Print description - log.Info("Tutorial description:"); - PrintDescription(); + spdlog::info( + ">>> Tutorial description <<<\nThis tutorial runs real-time Cartesian-space unified " + "motion-force control. The Z axis of the chosen reference frame will be activated for " + "explicit force control, while the rest axes in the same reference frame will stay motion " + "controlled."); // The reference frame to use for force control, see Robot::SetForceControlFrame() for more // details std::string frame_str = "WORLD"; - if (flexiv::utility::ProgramArgsExist(argc, argv, "--TCP")) { - log.Info("Reference frame used for force control: robot TCP frame"); + if (flexiv::rdk::utility::ProgramArgsExist(argc, argv, "--TCP")) { + spdlog::info("Reference frame used for force control: robot TCP frame"); frame_str = "TCP"; } else { - log.Info("Reference frame used for force control: robot world frame"); + spdlog::info("Reference frame used for force control: robot world frame"); } // Whether to enable polish motion bool enable_polish = false; - if (flexiv::utility::ProgramArgsExist(argc, argv, "--polish")) { - log.Info("Robot will run a polish motion along XY plane in robot world frame"); + if (flexiv::rdk::utility::ProgramArgsExist(argc, argv, "--polish")) { + spdlog::info("Robot will run a polish motion along XY plane in robot world frame"); enable_polish = true; } else { - log.Info("Robot will hold its motion in all non-force-controlled axes"); + spdlog::info("Robot will hold its motion in all non-force-controlled axes"); } try { // RDK Initialization // ========================================================================================= // Instantiate robot interface - flexiv::Robot robot(robot_sn); + flexiv::rdk::Robot robot(robot_sn); // Clear fault on the connected robot if any if (robot.fault()) { - log.Warn("Fault occurred on the connected robot, trying to clear ..."); + spdlog::warn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault if (!robot.ClearFault()) { - log.Error("Fault cannot be cleared, exiting ..."); + spdlog::error("Fault cannot be cleared, exiting ..."); return 1; } - log.Info("Fault on the connected robot is cleared"); + spdlog::info("Fault on the connected robot is cleared"); } // Enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ..."); + spdlog::info("Enabling robot ..."); robot.Enable(); // Wait for the robot to become operational while (!robot.operational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); } - log.Info("Robot is now operational"); + spdlog::info("Robot is now operational"); // Move robot to home pose - log.Info("Moving to home pose"); - robot.SwitchMode(flexiv::Mode::NRT_PRIMITIVE_EXECUTION); + spdlog::info("Moving to home pose"); + robot.SwitchMode(flexiv::rdk::Mode::NRT_PRIMITIVE_EXECUTION); robot.ExecutePrimitive("Home()"); // Wait for the primitive to finish @@ -203,28 +193,29 @@ int main(int argc, char* argv[]) // WARNING: during the process, the robot must not contact anything, otherwise the result // will be inaccurate and affect following operations - log.Warn("Zeroing force/torque sensors, make sure nothing is in contact with the robot"); + spdlog::warn( + "Zeroing force/torque sensors, make sure nothing is in contact with the robot"); // Wait for primitive completion while (robot.busy()) { std::this_thread::sleep_for(std::chrono::seconds(1)); } - log.Info("Sensor zeroing complete"); + spdlog::info("Sensor zeroing complete"); // Search for Contact // ========================================================================================= // NOTE: there are several ways to do contact search, such as using primitives, or real-time // and non-real-time direct motion controls, etc. Here we use non-real-time direct Cartesian // control for example. - log.Info("Searching for contact ..."); + spdlog::info("Searching for contact ..."); // Set initial pose to current TCP pose auto init_pose = robot.states().tcp_pose; - log.Info("Initial TCP pose set to [position 3x1, rotation (quaternion) 4x1]: " - + flexiv::utility::Arr2Str(init_pose)); + spdlog::info("Initial TCP pose set to [position 3x1, rotation (quaternion) 4x1]: " + + flexiv::rdk::utility::Arr2Str(init_pose)); // Use non-real-time mode to make the robot go to a set point with its own motion generator - robot.SwitchMode(flexiv::Mode::NRT_CARTESIAN_MOTION_FORCE); + robot.SwitchMode(flexiv::rdk::Mode::NRT_CARTESIAN_MOTION_FORCE); // Search for contact with max contact wrench set to a small value for making soft contact robot.SetMaxContactWrench(kMaxWrenchForContactSearch); @@ -247,7 +238,7 @@ int main(int argc, char* argv[]) // Contact is considered to be made if sensed TCP force exceeds the threshold if (ext_force.norm() > kPressingForce) { is_contacted = true; - log.Info("Contact detected at robot TCP"); + spdlog::info("Contact detected at robot TCP"); } // Check at 1ms interval std::this_thread::sleep_for(std::chrono::milliseconds(1)); @@ -265,7 +256,7 @@ int main(int argc, char* argv[]) // Set which Cartesian axis(s) to activate for force control. See function doc for more // details. Here we only active Z axis robot.SetForceControlAxis( - std::array {false, false, true, false, false, false}); + std::array {false, false, true, false, false, false}); // Uncomment the following line to enable passive force control, otherwise active force // control is used by default. See function doc for more details @@ -277,7 +268,7 @@ int main(int argc, char* argv[]) // Start Unified Motion Force Control // ========================================================================================= // Switch to real-time mode for continuous motion force control - robot.SwitchMode(flexiv::Mode::RT_CARTESIAN_MOTION_FORCE); + robot.SwitchMode(flexiv::rdk::Mode::RT_CARTESIAN_MOTION_FORCE); // Disable max contact wrench regulation. Need to do this AFTER the force control in Z axis // is activated (i.e. motion control disabled in Z axis) and the motion force control mode @@ -289,10 +280,10 @@ int main(int argc, char* argv[]) init_pose = robot.states().tcp_pose; // Create real-time scheduler to run periodic tasks - flexiv::Scheduler scheduler; + flexiv::rdk::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority - scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::ref(log), - std::ref(init_pose), std::ref(frame_str), enable_polish), + scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::ref(init_pose), + std::ref(frame_str), enable_polish), "HP periodic", 1, scheduler.max_priority()); // Start all added tasks scheduler.Start(); @@ -305,7 +296,7 @@ int main(int argc, char* argv[]) scheduler.Stop(); } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); return 1; } diff --git a/example/intermediate6_robot_dynamics.cpp b/example/intermediate6_robot_dynamics.cpp deleted file mode 100644 index a0cee2e0..00000000 --- a/example/intermediate6_robot_dynamics.cpp +++ /dev/null @@ -1,172 +0,0 @@ -/** - * @example intermediate6_robot_dynamics.cpp - * This tutorial runs the integrated dynamics engine to obtain robot Jacobian, mass matrix, and - * gravity force. - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. - * @author Flexiv - */ - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -/** @brief Print tutorial description */ -void PrintDescription() -{ - std::cout << "This tutorial runs the integrated dynamics engine to obtain robot Jacobian, mass " - "matrix, and gravity force." - << std::endl - << std::endl; -} - -/** @brief Print program usage help */ -void PrintHelp() -{ - // clang-format off - std::cout << "Required arguments: [robot SN]" << std::endl; - std::cout << " robot SN: Serial number of the robot to connect to. " - "Remove any space, for example: Rizon4s-123456" << std::endl; - std::cout << "Optional arguments: None" << std::endl; - std::cout << std::endl; - // clang-format on -} - -/** @brief Periodic task running at 100 Hz */ -int PeriodicTask(flexiv::Robot& robot, flexiv::Model& model) -{ - // Logger for printing message with timestamp and coloring - flexiv::Log log; - - // Local periodic loop counter - uint64_t loop_counter = 0; - - try { - while (true) { - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - loop_counter++; - - // Monitor fault on the connected robot - if (robot.fault()) { - throw std::runtime_error( - "PeriodicTask: Fault occurred on the connected robot, exiting ..."); - } - - // Mark timer start point - auto tic = std::chrono::high_resolution_clock::now(); - - // Update robot model in dynamics engine - model.Update(robot.states().q, robot.states().dtheta); - - // Compute gravity vector - auto g = model.g(); - - // Compute mass matrix - auto M = model.M(); - - // Compute Jacobian - auto J = model.J("flange"); - - // Mark timer end point and get loop time - auto toc = std::chrono::high_resolution_clock::now(); - auto computation_time - = std::chrono::duration_cast(toc - tic).count(); - - // Print at 1Hz - if (loop_counter % 100 == 0) { - // Print time used to compute g, M, J - log.Info("Computation time = " + std::to_string(computation_time) + " us"); - std::cout << std::endl; - // Print gravity - std::cout << std::fixed << std::setprecision(5) << "g = " << g.transpose() << "\n" - << std::endl; - // Print mass matrix - std::cout << std::fixed << std::setprecision(5) << "M = " << M << "\n" << std::endl; - // Print Jacobian - std::cout << std::fixed << std::setprecision(5) << "J = " << J << "\n" << std::endl; - } - } - } catch (const std::exception& e) { - log.Error(e.what()); - return 1; - } -} - -int main(int argc, char* argv[]) -{ - // Program Setup - // ============================================================================================= - // Logger for printing message with timestamp and coloring - flexiv::Log log; - - // Parse parameters - if (argc < 2 || flexiv::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { - PrintHelp(); - return 1; - } - // Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456 - std::string robot_sn = argv[1]; - - // Print description - log.Info("Tutorial description:"); - PrintDescription(); - - try { - // RDK Initialization - // ========================================================================================= - // Instantiate robot interface - flexiv::Robot robot(robot_sn); - - // Clear fault on the connected robot if any - if (robot.fault()) { - log.Warn("Fault occurred on the connected robot, trying to clear ..."); - // Try to clear the fault - if (!robot.ClearFault()) { - log.Error("Fault cannot be cleared, exiting ..."); - return 1; - } - log.Info("Fault on the connected robot is cleared"); - } - - // Enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ..."); - robot.Enable(); - - // Wait for the robot to become operational - while (!robot.operational()) { - std::this_thread::sleep_for(std::chrono::seconds(1)); - } - log.Info("Robot is now operational"); - - // Move robot to home pose - log.Info("Moving to home pose"); - robot.SwitchMode(flexiv::Mode::NRT_PRIMITIVE_EXECUTION); - robot.ExecutePrimitive("Home()"); - - // Wait for the primitive to finish - while (robot.busy()) { - std::this_thread::sleep_for(std::chrono::seconds(1)); - } - - // Robot Dynamics - // ========================================================================================= - // Initialize dynamics engine - flexiv::Model model(robot); - - // Use std thread for periodic task so this example can run on Windows - std::thread periodic_task_thread(PeriodicTask, std::ref(robot), std::ref(model)); - periodic_task_thread.join(); - - } catch (const std::exception& e) { - log.Error(e.what()); - return 1; - } - - return 0; -} diff --git a/example/intermediate7_robot_dynamics.cpp b/example/intermediate7_robot_dynamics.cpp new file mode 100644 index 00000000..a47a092d --- /dev/null +++ b/example/intermediate7_robot_dynamics.cpp @@ -0,0 +1,141 @@ +/** + * @example intermediate7_robot_dynamics.cpp + * This tutorial runs the integrated dynamics engine to obtain robot Jacobian, mass matrix, and + * gravity torques. Also checks reachability of a Cartesian pose. + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. + * @author Flexiv + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +/** @brief Print program usage help */ +void PrintHelp() +{ + // clang-format off + std::cout << "Required arguments: [robot SN]" << std::endl; + std::cout << " robot SN: Serial number of the robot to connect to. " + "Remove any space, for example: Rizon4s-123456" << std::endl; + std::cout << "Optional arguments: None" << std::endl; + std::cout << std::endl; + // clang-format on +} + +int main(int argc, char* argv[]) +{ + // Program Setup + // ============================================================================================= + // Parse parameters + if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { + PrintHelp(); + return 1; + } + // Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456 + std::string robot_sn = argv[1]; + + // Print description + spdlog::info( + ">>> Tutorial description <<<\nThis tutorial runs the integrated dynamics engine to obtain " + "robot Jacobian, mass matrix, and gravity torques. Also checks reachability of a Cartesian " + "pose."); + + try { + // RDK Initialization + // ========================================================================================= + // Instantiate robot interface + flexiv::rdk::Robot robot(robot_sn); + + // Clear fault on the connected robot if any + if (robot.fault()) { + spdlog::warn("Fault occurred on the connected robot, trying to clear ..."); + // Try to clear the fault + if (!robot.ClearFault()) { + spdlog::error("Fault cannot be cleared, exiting ..."); + return 1; + } + spdlog::info("Fault on the connected robot is cleared"); + } + + // Enable the robot, make sure the E-stop is released before enabling + spdlog::info("Enabling robot ..."); + robot.Enable(); + + // Wait for the robot to become operational + while (!robot.operational()) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + spdlog::info("Robot is now operational"); + + // Move robot to home pose + spdlog::info("Moving to home pose"); + robot.SwitchMode(flexiv::rdk::Mode::NRT_PRIMITIVE_EXECUTION); + robot.ExecutePrimitive("Home()"); + + // Wait for the primitive to finish + while (robot.busy()) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + // Robot Dynamics + // ========================================================================================= + // Initialize dynamics engine + flexiv::rdk::Model model(robot); + + // Step dynamics engine 5 times + for (size_t i = 0; i < 5; i++) { + // Mark timer start point + auto tic = std::chrono::high_resolution_clock::now(); + + // Update robot model in dynamics engine + model.Update(robot.states().q, robot.states().dtheta); + + // Compute gravity vector + auto g = model.g(); + + // Compute mass matrix + auto M = model.M(); + + // Compute Jacobian + auto J = model.J("flange"); + + // Mark timer end point and get loop time + auto toc = std::chrono::high_resolution_clock::now(); + auto computation_time + = std::chrono::duration_cast(toc - tic).count(); + + // Print time used to compute g, M, J + spdlog::info("Computation time = {} us", computation_time); + // Print gravity + std::cout << "g = \n" + << std::fixed << std::setprecision(5) << g.transpose() << std::endl; + // Print mass matrix + std::cout << "M = \n" << std::fixed << std::setprecision(5) << M << std::endl; + // Print Jacobian + std::cout << "J = \n" << std::fixed << std::setprecision(5) << J << std::endl; + std::cout << std::endl; + } + + // Check reachability of a Cartesian pose based on current pose + auto pose_to_check = robot.states().tcp_pose; + pose_to_check[0] += 0.1; + spdlog::info("Checking reachability of Cartesian pose [{}]", + flexiv::rdk::utility::Arr2Str(pose_to_check)); + auto result = model.reachable(pose_to_check, robot.states().q, true); + spdlog::info("Got a result: reachable = {}, IK solution = [{}]", result.first, + flexiv::rdk::utility::Vec2Str(result.second)); + + } catch (const std::exception& e) { + spdlog::error(e.what()); + return 1; + } + + return 0; +} diff --git a/example_py/basics1_display_robot_states.py b/example_py/basics1_display_robot_states.py index 52135ce4..af2a0e59 100644 --- a/example_py/basics1_display_robot_states.py +++ b/example_py/basics1_display_robot_states.py @@ -6,65 +6,53 @@ received robot states. """ -__copyright__ = "Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved." +__copyright__ = "Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved." __author__ = "Flexiv" import time import argparse import threading +import spdlog # pip install spdlog -# Import Flexiv RDK Python library -# fmt: off -import sys -sys.path.insert(0, "../lib_py") +# Flexiv RDK Python library is installed to user site packages import flexivrdk -# fmt: on -def print_description(): - """ - Print tutorial description. - - """ - print( - "This tutorial does the very first thing: check connection with the robot server " - "and print received robot states." - ) - print() - - -def print_robot_states(robot, log): +def print_robot_states(robot, logger, stop_event): """ Print robot states data @ 1Hz. """ - while True: + while not stop_event.is_set(): # Print all gripper states, round all float values to 2 decimals - log.Info("Current robot states:") + logger.info("Current robot states:") # fmt: off print("{") - print("q: ", ['%.2f' % i for i in robot.states().q]) - print("theta: ", ['%.2f' % i for i in robot.states().theta]) - print("dq: ", ['%.2f' % i for i in robot.states().dq]) - print("dtheta: ", ['%.2f' % i for i in robot.states().dtheta]) - print("tau: ", ['%.2f' % i for i in robot.states().tau]) - print("tau_des: ", ['%.2f' % i for i in robot.states().tau_des]) - print("tau_dot: ", ['%.2f' % i for i in robot.states().tau_dot]) - print("tau_ext: ", ['%.2f' % i for i in robot.states().tau_ext]) - print("tcp_pose: ", ['%.2f' % i for i in robot.states().tcp_pose]) - print("tcp_pose_d: ", ['%.2f' % i for i in robot.states().tcp_pose_des]) - print("tcp_velocity: ", ['%.2f' % i for i in robot.states().tcp_vel]) - print("flange_pose: ", ['%.2f' % i for i in robot.states().flange_pose]) - print("FT_sensor_raw_reading: ", ['%.2f' % i for i in robot.states().ft_sensor_raw]) - print("F_ext_tcp_frame: ", ['%.2f' % i for i in robot.states().ext_wrench_in_tcp]) - print("F_ext_world_frame: ", ['%.2f' % i for i in robot.states().ext_wrench_in_world]) - print("}") + print(f"q: {['%.2f' % i for i in robot.states().q]}",) + print(f"theta: {['%.2f' % i for i in robot.states().theta]}") + print(f"dq: {['%.2f' % i for i in robot.states().dq]}") + print(f"dtheta: {['%.2f' % i for i in robot.states().dtheta]}") + print(f"tau: {['%.2f' % i for i in robot.states().tau]}") + print(f"tau_des: {['%.2f' % i for i in robot.states().tau_des]}") + print(f"tau_dot: {['%.2f' % i for i in robot.states().tau_dot]}") + print(f"tau_ext: {['%.2f' % i for i in robot.states().tau_ext]}") + print(f"tcp_pose: {['%.2f' % i for i in robot.states().tcp_pose]}") + print(f"tcp_pose_d: {['%.2f' % i for i in robot.states().tcp_pose_des]}") + print(f"tcp_velocity: {['%.2f' % i for i in robot.states().tcp_vel]}") + print(f"flange_pose: {['%.2f' % i for i in robot.states().flange_pose]}") + print(f"FT_sensor_raw_reading: {['%.2f' % i for i in robot.states().ft_sensor_raw]}") + print(f"F_ext_tcp_frame: {['%.2f' % i for i in robot.states().ext_wrench_in_tcp]}") + print(f"F_ext_world_frame: {['%.2f' % i for i in robot.states().ext_wrench_in_world]}") + print("}", flush= True) # fmt: on time.sleep(1) def main(): + # Create an event to signal the thread to stop + stop_event = threading.Event() + # Program Setup # ============================================================================================== # Parse arguments @@ -76,12 +64,14 @@ def main(): args = argparser.parse_args() # Define alias - log = flexivrdk.Log() + logger = spdlog.ConsoleLogger("Example") mode = flexivrdk.Mode # Print description - log.Info("Tutorial description:") - print_description() + logger.info( + ">>> Tutorial description <<<\nThis tutorial does the very first thing: check connection " + "with the robot server and print received robot states." + ) try: # RDK Initialization @@ -91,33 +81,47 @@ def main(): # Clear fault on the connected robot if any if robot.fault(): - log.Warn("Fault occurred on the connected robot, trying to clear ...") + logger.warn("Fault occurred on the connected robot, trying to clear ...") # Try to clear the fault if not robot.ClearFault(): - log.Error("Fault cannot be cleared, exiting ...") + logger.error("Fault cannot be cleared, exiting ...") return 1 - log.Info("Fault on the connected robot is cleared") + logger.info("Fault on the connected robot is cleared") # Enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ...") + logger.info("Enabling robot ...") robot.Enable() # Wait for the robot to become operational while not robot.operational(): time.sleep(1) - log.Info("Robot is now operational") - - # Print States - # ============================================================================= - # Thread for printing robot states - print_thread = threading.Thread(target=print_robot_states, args=[robot, log]) - print_thread.start() - print_thread.join() + logger.info("Robot is now operational") except Exception as e: # Print exception error message - log.Error(str(e)) + logger.error(str(e)) + + # Print States + # ============================================================================= + # Thread for printing robot states + print_thread = threading.Thread( + target=print_robot_states, args=[robot, logger, stop_event] + ) + print_thread.start() + + # Use main thread to catch keyboard interrupt and exit thread + try: + while not stop_event.is_set(): + time.sleep(0.1) + except KeyboardInterrupt: + # Send signal to exit thread + logger.info("Stopping print thread") + stop_event.set() + + # Wait for thread to exit + print_thread.join() + logger.info("Print thread exited") if __name__ == "__main__": diff --git a/example_py/basics2_clear_fault.py b/example_py/basics2_clear_fault.py index 3769df21..d0ef0e1d 100644 --- a/example_py/basics2_clear_fault.py +++ b/example_py/basics2_clear_fault.py @@ -2,34 +2,18 @@ """basics2_clear_fault.py -This tutorial clears minor faults from the robot server if any. Note that critical faults cannot -be cleared, see RDK manual for more details. +This tutorial clears minor or critical faults, if any, of the connected robot. """ -__copyright__ = "Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved." +__copyright__ = "Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved." __author__ = "Flexiv" import time import argparse +import spdlog # pip install spdlog -# Import Flexiv RDK Python library -# fmt: off -import sys -sys.path.insert(0, "../lib_py") +# Flexiv RDK Python library is installed to user site packages import flexivrdk -# fmt: on - - -def print_description(): - """ - Print tutorial description. - - """ - print( - "This tutorial clears minor faults from the robot server if any. Note that " - "critical faults cannot be cleared, see RDK manual for more details." - ) - print() def main(): @@ -44,11 +28,13 @@ def main(): args = argparser.parse_args() # Define alias - log = flexivrdk.Log() + logger = spdlog.ConsoleLogger("Example") # Print description - log.Info("Tutorial description:") - print_description() + logger.info( + ">>> Tutorial description <<<\nThis tutorial clears minor or critical faults, if any, of " + "the connected robot." + ) try: # RDK Initialization @@ -60,18 +46,18 @@ def main(): # ========================================================================================== # Clear fault on the connected robot if any if robot.fault(): - log.Warn("Fault occurred on the connected robot, trying to clear ...") + logger.warn("Fault occurred on the connected robot, trying to clear ...") # Try to clear the fault if not robot.ClearFault(): - log.Error("Fault cannot be cleared, exiting ...") + logger.error("Fault cannot be cleared, exiting ...") return 1 - log.Info("Fault on the connected robot is cleared") + logger.info("Fault on the connected robot is cleared") else: - log.Info("No fault on the connected robot") + logger.info("No fault on the connected robot") except Exception as e: # Print exception error message - log.Error(str(e)) + logger.error(str(e)) if __name__ == "__main__": diff --git a/example_py/basics3_primitive_execution.py b/example_py/basics3_primitive_execution.py index db71dae9..1b96172f 100644 --- a/example_py/basics3_primitive_execution.py +++ b/example_py/basics3_primitive_execution.py @@ -6,36 +6,20 @@ on all available primitives, please see [Flexiv Primitives](https://www.flexiv.com/primitives/). """ -__copyright__ = "Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved." +__copyright__ = "Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved." __author__ = "Flexiv" import time import argparse +import spdlog # pip install spdlog # Utility methods from utility import quat2eulerZYX from utility import parse_pt_states from utility import list2str -# Import Flexiv RDK Python library -# fmt: off -import sys -sys.path.insert(0, "../lib_py") +# Flexiv RDK Python library is installed to user site packages import flexivrdk -# fmt: on - - -def print_description(): - """ - Print tutorial description. - - """ - print( - "This tutorial executes several basic robot primitives (unit skills). For " - "detailed documentation on all available primitives, please see [Flexiv " - "Primitives](https://www.flexiv.com/primitives/)." - ) - print() def main(): @@ -50,12 +34,15 @@ def main(): args = argparser.parse_args() # Define alias - log = flexivrdk.Log() + logger = spdlog.ConsoleLogger("Example") mode = flexivrdk.Mode # Print description - log.Info("Tutorial description:") - print_description() + logger.info( + ">>> Tutorial description <<<\nThis tutorial executes several basic robot primitives (unit " + "skills). For detailed documentation on all available primitives, please see [Flexiv " + "Primitives](https://www.flexiv.com/primitives/)." + ) try: # RDK Initialization @@ -65,22 +52,22 @@ def main(): # Clear fault on the connected robot if any if robot.fault(): - log.Warn("Fault occurred on the connected robot, trying to clear ...") + logger.warn("Fault occurred on the connected robot, trying to clear ...") # Try to clear the fault if not robot.ClearFault(): - log.Error("Fault cannot be cleared, exiting ...") + logger.error("Fault cannot be cleared, exiting ...") return 1 - log.Info("Fault on the connected robot is cleared") + logger.info("Fault on the connected robot is cleared") # Enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ...") + logger.info("Enabling robot ...") robot.Enable() # Wait for the robot to become operational while not robot.operational(): time.sleep(1) - log.Info("Robot is now operational") + logger.info("Robot is now operational") # Execute Primitives # ========================================================================================== @@ -91,7 +78,7 @@ def main(): # ------------------------------------------------------------------------------------------ # All parameters of the "Home" primitive are optional, thus we can skip the parameters and # the default values will be used - log.Info("Executing primitive: Home") + logger.info("Executing primitive: Home") # Send command to robot robot.ExecutePrimitive("Home()") @@ -103,7 +90,7 @@ def main(): # (2) Move robot joints to target positions # ------------------------------------------------------------------------------------------ # The required parameter takes in 7 target joint positions. Unit: degrees - log.Info("Executing primitive: MoveJ") + logger.info("Executing primitive: MoveJ") # Send command to robot robot.ExecutePrimitive("MoveJ(target=30 -45 0 90 0 40 30)") @@ -121,15 +108,15 @@ def main(): # Optional parameter: # waypoints: waypoints to pass before reaching final target # (same format as above, but can repeat for number of waypoints) - # maxVel: maximum TCP linear velocity + # vel: TCP linear velocity # Unit: m/s # NOTE: The rotations use Euler ZYX convention, rot_x means Euler ZYX angle around X axis - log.Info("Executing primitive: MoveL") + logger.info("Executing primitive: MoveL") # Send command to robot robot.ExecutePrimitive( "MoveL(target=0.65 -0.3 0.2 180 0 180 WORLD WORLD_ORIGIN,waypoints=0.45 0.1 0.2 180 0 " - "180 WORLD WORLD_ORIGIN : 0.45 -0.3 0.2 180 0 180 WORLD WORLD_ORIGIN, maxVel=0.2)" + "180 WORLD WORLD_ORIGIN : 0.45 -0.3 0.2 180 0 180 WORLD WORLD_ORIGIN, vel=0.2)" ) # The [Move] series primitive won't terminate itself, so we determine if the robot has @@ -143,7 +130,7 @@ def main(): # ------------------------------------------------------------------------------------------ # In this example the reference frame is changed from WORLD::WORLD_ORIGIN to TRAJ::START, # which represents the current TCP frame - log.Info("Executing primitive: MoveL") + logger.info("Executing primitive: MoveL") # Example to convert target quaternion [w,x,y,z] to Euler ZYX using scipy package's 'xyz' # extrinsic rotation @@ -166,7 +153,7 @@ def main(): except Exception as e: # Print exception error message - log.Error(str(e)) + logger.error(str(e)) if __name__ == "__main__": diff --git a/example_py/basics4_plan_execution.py b/example_py/basics4_plan_execution.py index 33929a0c..1073a95d 100644 --- a/example_py/basics4_plan_execution.py +++ b/example_py/basics4_plan_execution.py @@ -8,33 +8,15 @@ to the robot, which will appear in the plan list. """ -__copyright__ = "Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved." +__copyright__ = "Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved." __author__ = "Flexiv" import time import argparse +import spdlog # pip install spdlog -# Import Flexiv RDK Python library -# fmt: off -import sys -sys.path.insert(0, "../lib_py") +# Flexiv RDK Python library is installed to user site packages import flexivrdk -# fmt: on - - -def print_description(): - """ - Print tutorial description. - - """ - print( - "This tutorial executes a plan selected by the user from a list of available " - "plans. A plan is a pre-written script to execute a series of robot primitives " - "with pre-defined transition conditions between 2 adjacent primitives. Users can " - "use Flexiv Elements to compose their own plan and assign to the robot, which " - "will appear in the plan list." - ) - print() def main(): @@ -49,12 +31,17 @@ def main(): args = argparser.parse_args() # Define alias - log = flexivrdk.Log() + logger = spdlog.ConsoleLogger("Example") mode = flexivrdk.Mode # Print description - log.Info("Tutorial description:") - print_description() + logger.info( + ">>> Tutorial description <<<\nThis tutorial executes a plan selected by the user from a " + "list of available plans. A plan is a pre-written script to execute a series of robot " + "primitives with pre-defined transition conditions between 2 adjacent primitives. Users " + "can use Flexiv Elements to compose their own plan and assign to the robot, which " + "will appear in the plan list." + ) try: # RDK Initialization @@ -64,22 +51,22 @@ def main(): # Clear fault on the connected robot if any if robot.fault(): - log.Warn("Fault occurred on the connected robot, trying to clear ...") + logger.warn("Fault occurred on the connected robot, trying to clear ...") # Try to clear the fault if not robot.ClearFault(): - log.Error("Fault cannot be cleared, exiting ...") + logger.error("Fault cannot be cleared, exiting ...") return 1 - log.Info("Fault on the connected robot is cleared") + logger.info("Fault on the connected robot is cleared") # Enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ...") + logger.info("Enabling robot ...") robot.Enable() # Wait for the robot to become operational while not robot.operational(): time.sleep(1) - log.Info("Robot is now operational") + logger.info("Robot is now operational") # Execute Plans # ========================================================================================== @@ -92,7 +79,7 @@ def main(): raise Exception("Fault occurred on the connected robot, exiting ...") # Get user input - log.Info("Choose an action:") + logger.info("Choose an action:") print("[1] Show available plans") print("[2] Execute a plan by index") print("[3] Execute a plan by name") @@ -102,7 +89,7 @@ def main(): if user_input == 1: plan_list = robot.plan_list() for i in range(len(plan_list)): - print("[" + str(i) + "]", plan_list[i]) + print(f"[{i}] {plan_list[i]}") print("") # Execute plan by index @@ -115,16 +102,16 @@ def main(): # Print plan info while the current plan is running while robot.busy(): plan_info = robot.plan_info() - log.Info(" ") - print("assigned_plan_name: ", plan_info.assigned_plan_name) - print("pt_name: ", plan_info.pt_name) - print("node_name: ", plan_info.node_name) - print("node_path: ", plan_info.node_path) - print("node_path_time_period: ", plan_info.node_path_time_period) - print("node_path_number: ", plan_info.node_path_number) - print("velocity_scale: ", plan_info.velocity_scale) - print("waiting_for_step: ", plan_info.waiting_for_step) - print("") + logger.info("Current plan info:") + print(f"assigned_plan_name: {plan_info.assigned_plan_name}") + print(f"pt_name: {plan_info.pt_name}") + print(f"node_name: {plan_info.node_name}") + print(f"node_path: {plan_info.node_path}") + print(f"node_path_time_period: {plan_info.node_path_time_period}") + print(f"node_path_number: {plan_info.node_path_number}") + print(f"velocity_scale: {plan_info.velocity_scale}") + print(f"waiting_for_step: {plan_info.waiting_for_step}") + print("", flush=True) time.sleep(1) # Execute plan by name @@ -137,24 +124,24 @@ def main(): # Print plan info while the current plan is running while robot.busy(): plan_info = robot.plan_info() - log.Info(" ") - print("assigned_plan_name: ", plan_info.assigned_plan_name) - print("pt_name: ", plan_info.pt_name) - print("node_name: ", plan_info.node_name) - print("node_path: ", plan_info.node_path) - print("node_path_time_period: ", plan_info.node_path_time_period) - print("node_path_number: ", plan_info.node_path_number) - print("velocity_scale: ", plan_info.velocity_scale) - print("waiting_for_step: ", plan_info.waiting_for_step) - print("") + logger.info("Current plan info:") + print(f"assigned_plan_name: {plan_info.assigned_plan_name}") + print(f"pt_name: {plan_info.pt_name}") + print(f"node_name: {plan_info.node_name}") + print(f"node_path: {plan_info.node_path}") + print(f"node_path_time_period: {plan_info.node_path_time_period}") + print(f"node_path_number: {plan_info.node_path_number}") + print(f"velocity_scale: {plan_info.velocity_scale}") + print(f"waiting_for_step: {plan_info.waiting_for_step}") + print("", flush=True) time.sleep(1) else: - log.Warn("Invalid input") + logger.warn("Invalid input") except Exception as e: # Print exception error message - log.Error(str(e)) + logger.error(str(e)) if __name__ == "__main__": diff --git a/example_py/basics5_zero_force_torque_sensors.py b/example_py/basics5_zero_force_torque_sensors.py index 6988c8c8..33f721b2 100644 --- a/example_py/basics5_zero_force_torque_sensors.py +++ b/example_py/basics5_zero_force_torque_sensors.py @@ -6,32 +6,16 @@ mandatory) step before any operations that require accurate force/torque measurement. """ -__copyright__ = "Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved." +__copyright__ = "Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved." __author__ = "Flexiv" import time import argparse +import spdlog # pip install spdlog from utility import list2str -# Import Flexiv RDK Python library -# fmt: off -import sys -sys.path.insert(0, "../lib_py") +# Flexiv RDK Python library is installed to user site packages import flexivrdk -# fmt: on - - -def print_description(): - """ - Print tutorial description. - - """ - print( - "This tutorial zeros the robot's force and torque sensors, which is a recommended " - "(but not mandatory) step before any operations that require accurate " - "force/torque measurement." - ) - print() def main(): @@ -46,12 +30,15 @@ def main(): args = argparser.parse_args() # Define alias - log = flexivrdk.Log() + logger = spdlog.ConsoleLogger("Example") mode = flexivrdk.Mode # Print description - log.Info("Tutorial description:") - print_description() + logger.info( + ">>> Tutorial description <<<\nThis tutorial zeros the robot's force and torque sensors, " + "which is a recommended (but not mandatory) step before any operations that require " + "accurate force/torque measurement." + ) try: # RDK Initialization @@ -61,30 +48,28 @@ def main(): # Clear fault on the connected robot if any if robot.fault(): - log.Warn("Fault occurred on the connected robot, trying to clear ...") + logger.warn("Fault occurred on the connected robot, trying to clear ...") # Try to clear the fault if not robot.ClearFault(): - log.Error("Fault cannot be cleared, exiting ...") + logger.error("Fault cannot be cleared, exiting ...") return 1 - log.Info("Fault on the connected robot is cleared") + logger.info("Fault on the connected robot is cleared") # Enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ...") + logger.info("Enabling robot ...") robot.Enable() # Wait for the robot to become operational while not robot.operational(): time.sleep(1) - log.Info("Robot is now operational") + logger.info("Robot is now operational") # Zero Sensors # ========================================================================================== # Get and print the current TCP force/moment readings - log.Info( - "TCP force and moment reading in base frame BEFORE sensor zeroing: " - + list2str(robot.states().ext_wrench_in_world) - + "[N][Nm]" + logger.info( + f"TCP force and moment reading in base frame BEFORE sensor zeroing: {robot.states().ext_wrench_in_world} N-Nm" ) # Run the "ZeroFTSensor" primitive to automatically zero force and torque sensors @@ -93,25 +78,23 @@ def main(): # WARNING: during the process, the robot must not contact anything, otherwise the result # will be inaccurate and affect following operations - log.Warn( + logger.warn( "Zeroing force/torque sensors, make sure nothing is in contact with the robot" ) # Wait for the primitive completion while robot.busy(): time.sleep(1) - log.Info("Sensor zeroing complete") + logger.info("Sensor zeroing complete") # Get and print the current TCP force/moment readings - log.Info( - "TCP force and moment reading in base frame AFTER sensor zeroing: " - + list2str(robot.states().ext_wrench_in_world) - + "[N][Nm]" + logger.info( + f"TCP force and moment reading in base frame AFTER sensor zeroing: {robot.states().ext_wrench_in_world} N-Nm" ) except Exception as e: # Print exception error message - log.Error(str(e)) + logger.error(str(e)) if __name__ == "__main__": diff --git a/example_py/basics6_gripper_control.py b/example_py/basics6_gripper_control.py index c5550a50..49ba5c2c 100644 --- a/example_py/basics6_gripper_control.py +++ b/example_py/basics6_gripper_control.py @@ -2,55 +2,41 @@ """basics6_gripper_control.py -This tutorial does position and force control (if available) for grippers supported by Flexiv. +This tutorial does position and force (if available) control of grippers supported by Flexiv. """ -__copyright__ = "Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved." +__copyright__ = "Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved." __author__ = "Flexiv" import time import argparse import threading +import spdlog # pip install spdlog -# Import Flexiv RDK Python library -# fmt: off -import sys -sys.path.insert(0, "../lib_py") +# Flexiv RDK Python library is installed to user site packages import flexivrdk -# fmt: on -# Global flag: whether the gripper control tasks are finished -g_is_done = False - -def print_description(): - """ - Print tutorial description. - - """ - print( - "This tutorial does position and force control (if available) for grippers " - "supported by Flexiv." - ) - print() - - -def print_gripper_states(gripper, log): +def print_gripper_states(gripper, logger, stop_event): """ Print gripper states data @ 1Hz. """ - while not g_is_done: + while not stop_event.is_set(): # Print all gripper states, round all float values to 2 decimals - log.Info("Current gripper states:") - print("width: ", round(gripper.states().width, 2)) - print("force: ", round(gripper.states().force, 2)) - print("max_width: ", round(gripper.states().max_width, 2)) - print("moving: ", gripper.moving()) + logger.info("Current gripper states:") + print(f"width: {round(gripper.states().width, 2)}") + print(f"force: {round(gripper.states().force, 2)}") + print(f"max_width: {round(gripper.states().max_width, 2)}") + print(f"moving: {gripper.moving()}") + print("", flush=True) time.sleep(1) def main(): + # Create an event to signal the thread to stop + stop_event = threading.Event() + # Program Setup # ============================================================================================== # Parse arguments @@ -62,12 +48,14 @@ def main(): args = argparser.parse_args() # Define alias - log = flexivrdk.Log() + logger = spdlog.ConsoleLogger("Example") mode = flexivrdk.Mode # Print description - log.Info("Tutorial description:") - print_description() + logger.info( + ">>> Tutorial description <<<\nThis tutorial does position and force (if available) " + "control of grippers supported by Flexiv." + ) try: # RDK Initialization @@ -77,22 +65,22 @@ def main(): # Clear fault on the connected robot if any if robot.fault(): - log.Warn("Fault occurred on the connected robot, trying to clear ...") + logger.warn("Fault occurred on the connected robot, trying to clear ...") # Try to clear the fault if not robot.ClearFault(): - log.Error("Fault cannot be cleared, exiting ...") + logger.error("Fault cannot be cleared, exiting ...") return 1 - log.Info("Fault on the connected robot is cleared") + logger.info("Fault on the connected robot is cleared") # Enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ...") + logger.info("Enabling robot ...") robot.Enable() # Wait for the robot to become operational while not robot.operational(): time.sleep(1) - log.Info("Robot is now operational") + logger.info("Robot is now operational") # Gripper Control # ========================================================================================== @@ -106,57 +94,60 @@ def main(): gripper = flexivrdk.Gripper(robot) # Manually initialize the gripper, not all grippers need this step - log.Info("Initializing gripper, this process takes about 10 seconds ...") + logger.info("Initializing gripper, this process takes about 10 seconds ...") gripper.Init() - log.Info("Initialization complete") + logger.info("Initialization complete") # Thread for printing gripper states print_thread = threading.Thread( - target=print_gripper_states, args=[gripper, log] + target=print_gripper_states, args=[gripper, logger, stop_event] ) print_thread.start() # Position control - log.Info("Closing gripper") + logger.info("Closing gripper") gripper.Move(0.01, 0.1, 20) time.sleep(2) - log.Info("Opening gripper") + logger.info("Opening gripper") gripper.Move(0.09, 0.1, 20) time.sleep(2) # Stop - log.Info("Closing gripper") + logger.info("Closing gripper") gripper.Move(0.01, 0.1, 20) time.sleep(0.5) - log.Info("Stopping gripper") + logger.info("Stopping gripper") gripper.Stop() time.sleep(2) - log.Info("Closing gripper") + logger.info("Closing gripper") gripper.Move(0.01, 0.1, 20) time.sleep(2) - log.Info("Opening gripper") + logger.info("Opening gripper") gripper.Move(0.09, 0.1, 20) time.sleep(0.5) - log.Info("Stopping gripper") + logger.info("Stopping gripper") gripper.Stop() time.sleep(2) # Force control, if available (sensed force is not zero) if abs(gripper.states().force) > sys.float_info.epsilon: - log.Info("Gripper running zero force control") + logger.info("Gripper running zero force control") gripper.Grasp(0) # Exit after 10 seconds time.sleep(10) - # Finished, exit all threads + # Finished gripper.Stop() - global g_is_done - g_is_done = True - log.Info("Program finished") + + # Stop all threads + logger.info("Stopping print thread") + stop_event.set() print_thread.join() + logger.info("Print thread exited") + logger.info("Program finished") except Exception as e: - log.Error(str(e)) + logger.error(str(e)) if __name__ == "__main__": diff --git a/example_py/basics7_auto_recovery.py b/example_py/basics7_auto_recovery.py index eee32970..6b9c772d 100644 --- a/example_py/basics7_auto_recovery.py +++ b/example_py/basics7_auto_recovery.py @@ -3,33 +3,18 @@ """basics7_auto_recovery.py This tutorial runs an automatic recovery process if the robot's safety system is in recovery -state. See flexiv::Robot::recovery() and RDK manual for more details. +state. See Robot::recovery() and RDK manual for more details. """ -__copyright__ = "Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved." +__copyright__ = "Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved." __author__ = "Flexiv" import time import argparse +import spdlog # pip install spdlog -# Import Flexiv RDK Python library -# fmt: off -import sys -sys.path.insert(0, "../lib_py") +# Flexiv RDK Python library is installed to user site packages import flexivrdk -# fmt: on - - -def print_description(): - """ - Print tutorial description. - - """ - print( - "This tutorial runs an automatic recovery process if the robot's safety system is in " - "recovery state. See flexiv::Robot::recovery() and RDK manual for more details." - ) - print() def main(): @@ -44,11 +29,14 @@ def main(): args = argparser.parse_args() # Define alias - log = flexivrdk.Log() + logger = spdlog.ConsoleLogger("Example") # Print description - log.Info("Tutorial description:") - print_description() + logger.info( + ">>> Tutorial description <<<\nThis tutorial runs an automatic recovery process if the " + "robot's safety system is in recovery state. See flexiv::rdk::Robot::recovery() and RDK " + "manual for more details." + ) try: # RDK Initialization @@ -57,7 +45,7 @@ def main(): robot = flexivrdk.Robot(args.robot_sn) # Enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ...") + logger.info("Enabling robot ...") robot.Enable() # Run Auto-recovery @@ -72,13 +60,13 @@ def main(): robot.RunAutoRecovery() # Otherwise the system is normal, do nothing else: - log.Info( + logger.info( "Robot system is not in recovery state, nothing to be done, exiting ..." ) except Exception as e: # Print exception error message - log.Error(str(e)) + logger.error(str(e)) if __name__ == "__main__": diff --git a/example_py/basics8_update_robot_tool.py b/example_py/basics8_update_robot_tool.py index bbdceb5e..2a3ecf6f 100644 --- a/example_py/basics8_update_robot_tool.py +++ b/example_py/basics8_update_robot_tool.py @@ -7,31 +7,15 @@ must be put into IDLE mode when making these changes. """ -__copyright__ = "Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved." +__copyright__ = "Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved." __author__ = "Flexiv" import time import argparse +import spdlog # pip install spdlog -# Import Flexiv RDK Python library -# fmt: off -import sys -sys.path.insert(0, "../lib_py") +# Flexiv RDK Python library is installed to user site packages import flexivrdk -# fmt: on - - -def print_description(): - """ - Print tutorial description. - - """ - print( - "This tutorial shows how to online update and interact with the robot tools. All " - "changes made to the robot tool system will take effect immediately without needing to " - "reboot. However, the robot must be put into IDLE mode when making these changes." - ) - print() def main(): @@ -46,12 +30,16 @@ def main(): args = argparser.parse_args() # Define alias - log = flexivrdk.Log() + logger = spdlog.ConsoleLogger("Example") mode = flexivrdk.Mode # Print description - log.Info("Tutorial description:") - print_description() + logger.info( + ">>> Tutorial description <<<\nThis tutorial shows how to online update and interact with " + "the robot tools. All changes made to the robot tool system will take effect immediately " + "without needing to reboot. However, the robot must be put into IDLE mode when making " + "these changes." + ) try: # RDK Initialization @@ -61,22 +49,22 @@ def main(): # Clear fault on the connected robot if any if robot.fault(): - log.Warn("Fault occurred on the connected robot, trying to clear ...") + logger.warn("Fault occurred on the connected robot, trying to clear ...") # Try to clear the fault if not robot.ClearFault(): - log.Error("Fault cannot be cleared, exiting ...") + logger.error("Fault cannot be cleared, exiting ...") return 1 - log.Info("Fault on the connected robot is cleared") + logger.info("Fault on the connected robot is cleared") # Enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ...") + logger.info("Enabling robot ...") robot.Enable() # Wait for the robot to become operational while not robot.operational(): time.sleep(1) - log.Info("Robot is now operational") + logger.info("Robot is now operational") # Update Robot Tool # ========================================================================================== @@ -87,14 +75,14 @@ def main(): tool = flexivrdk.Tool(robot) # Get and print a list of already configured tools currently in the robot's tools pool - log.Info("All configured tools:") + logger.info("All configured tools:") tool_list = tool.list() for i in range(len(tool_list)): - print("[" + str(i) + "]", tool_list[i]) + print(f"[{i}] {tool_list[i]}") print() # Get and print the current active tool - log.Info("Current active tool: " + tool.name()) + logger.info(f"Current active tool: [{tool.name()}]") # Set name and parameters for a new tool new_tool_name = "ExampleTool1" @@ -115,45 +103,43 @@ def main(): # If there's already a tool with the same name in the robot's tools pool, then remove it # first, because duplicate tool names are not allowed if tool.exist(new_tool_name): - log.Warn( - "Tool with the same name [" - + new_tool_name - + "] already exists, removing it now" + logger.warn( + f"Tool with the same name [{new_tool_name}] already exists, removing it now" ) # Switch to other tool or no tool (Flange) before removing the current tool tool.Switch("Flange") tool.Remove(new_tool_name) # Add the new tool - log.Info("Adding new tool [" + new_tool_name + "] to the robot") + logger.info(f"Adding new tool [{new_tool_name}] to the robot") tool.Add(new_tool_name, new_tool_params) # Get and print the tools list again, the new tool should appear at the end - log.Info("All configured tools:") + logger.info("All configured tools:") tool_list = tool.list() for i in range(len(tool_list)): - print("[" + str(i) + "]", tool_list[i]) + print(f"[{i}] {tool_list[i]}") print() # Switch to the newly added tool, i.e. set it as the active tool - log.Info("Switching to tool [" + new_tool_name + "]") + logger.info(f"Switching to tool [{new_tool_name}]") tool.Switch(new_tool_name) # Get and print the current active tool again, should be the new tool - log.Info("Current active tool: " + tool.name()) + logger.info(f"Current active tool: [{tool.name()}]") # Switch to other tool or no tool (Flange) before removing the current tool tool.Switch("Flange") # Clean up by removing the new tool time.sleep(2) - log.Info("Removing tool [" + new_tool_name + "]") + logger.info(f"Removing tool [{new_tool_name}]") tool.Remove(new_tool_name) - log.Info("Program finished") + logger.info("Program finished") except Exception as e: - log.Error(str(e)) + logger.error(str(e)) if __name__ == "__main__": diff --git a/example_py/intermediate1_non_realtime_joint_position_control.py b/example_py/intermediate1_non_realtime_joint_position_control.py index e318e7e4..12e2192d 100644 --- a/example_py/intermediate1_non_realtime_joint_position_control.py +++ b/example_py/intermediate1_non_realtime_joint_position_control.py @@ -5,31 +5,16 @@ This tutorial runs non-real-time joint position control to hold or sine-sweep all robot joints. """ -__copyright__ = "Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved." +__copyright__ = "Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved." __author__ = "Flexiv" import time import math import argparse +import spdlog # pip install spdlog -# Import Flexiv RDK Python library -# fmt: off -import sys -sys.path.insert(0, "../lib_py") +# Flexiv RDK Python library is installed to user site packages import flexivrdk -# fmt: on - - -def print_description(): - """ - Print tutorial description. - - """ - print( - "This tutorial runs non-real-time joint position control to hold or sine-sweep all " - "robot joints." - ) - print() def main(): @@ -58,12 +43,14 @@ def main(): assert frequency >= 1 and frequency <= 100, "Invalid input" # Define alias - log = flexivrdk.Log() + logger = spdlog.ConsoleLogger("Example") mode = flexivrdk.Mode # Print description - log.Info("Tutorial description:") - print_description() + logger.info( + ">>> Tutorial description <<<\nThis tutorial runs non-real-time joint position control to " + "hold or sine-sweep all robot joints." + ) try: # RDK Initialization @@ -73,25 +60,25 @@ def main(): # Clear fault on the connected robot if any if robot.fault(): - log.Warn("Fault occurred on the connected robot, trying to clear ...") + logger.warn("Fault occurred on the connected robot, trying to clear ...") # Try to clear the fault if not robot.ClearFault(): - log.Error("Fault cannot be cleared, exiting ...") + logger.error("Fault cannot be cleared, exiting ...") return 1 - log.Info("Fault on the connected robot is cleared") + logger.info("Fault on the connected robot is cleared") # Enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ...") + logger.info("Enabling robot ...") robot.Enable() # Wait for the robot to become operational while not robot.operational(): time.sleep(1) - log.Info("Robot is now operational") + logger.info("Robot is now operational") # Move robot to home pose - log.Info("Moving to home pose") + logger.info("Moving to home pose") robot.SwitchMode(mode.NRT_PRIMITIVE_EXECUTION) robot.ExecutePrimitive("Home()") @@ -106,29 +93,25 @@ def main(): period = 1.0 / frequency loop_time = 0 - print( - "Sending command to robot at", - frequency, - "Hz, or", - period, - "seconds interval", + logger.info( + f"Sending command to robot at {frequency} Hz, or {period} seconds interval" ) # Use current robot joint positions as initial positions init_pos = robot.states().q.copy() - print("Initial positions set to: ", init_pos) + logger.info(f"Initial positions set to: {init_pos}") - # Robot degrees of freedom - DOF = len(robot.states().q) + # Robot joint degrees of freedom + DoF = robot.info().DoF # Initialize target vectors target_pos = init_pos.copy() - target_vel = [0.0] * DOF - target_acc = [0.0] * DOF + target_vel = [0.0] * DoF + target_acc = [0.0] * DoF # Joint motion constraints - MAX_VEL = [2.0] * DOF - MAX_ACC = [3.0] * DOF + MAX_VEL = [2.0] * DoF + MAX_ACC = [3.0] * DoF # Joint sine-sweep amplitude [rad] SWING_AMP = 0.1 @@ -147,7 +130,7 @@ def main(): # Sine-sweep all joints if not args.hold: - for i in range(DOF): + for i in range(DoF): target_pos[i] = init_pos[i] + SWING_AMP * math.sin( 2 * math.pi * SWING_FREQ * loop_time ) @@ -163,7 +146,7 @@ def main(): except Exception as e: # Print exception error message - log.Error(str(e)) + logger.error(str(e)) if __name__ == "__main__": diff --git a/example_py/intermediate2_non_realtime_joint_impedance_control.py b/example_py/intermediate2_non_realtime_joint_impedance_control.py new file mode 100644 index 00000000..3a139a62 --- /dev/null +++ b/example_py/intermediate2_non_realtime_joint_impedance_control.py @@ -0,0 +1,165 @@ +#!/usr/bin/env python + +"""intermediate2_non_realtime_joint_impedance_control.py + +This tutorial runs non-real-time joint impedance control to hold or sine-sweep all robot joints. +""" + +__copyright__ = "Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved." +__author__ = "Flexiv" + +import time +import math +import argparse +import spdlog # pip install spdlog +import numpy as np # pip install numpy + +# Flexiv RDK Python library is installed to user site packages +import flexivrdk + + +def main(): + # Program Setup + # ============================================================================================== + # Parse arguments + argparser = argparse.ArgumentParser() + # Required arguments + argparser.add_argument( + "robot_sn", + help="Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456", + ) + argparser.add_argument( + "frequency", help="Command frequency, 1 to 100 [Hz]", type=int + ) + # Optional arguments + argparser.add_argument( + "--hold", + action="store_true", + help="Robot holds current joint positions, otherwise do a sine-sweep", + ) + args = argparser.parse_args() + + # Check if arguments are valid + frequency = args.frequency + assert frequency >= 1 and frequency <= 100, "Invalid input" + + # Define alias + logger = spdlog.ConsoleLogger("Example") + mode = flexivrdk.Mode + + # Print description + logger.info( + ">>> Tutorial description <<<\nThis tutorial runs non-real-time joint impedance control to " + "hold or sine-sweep all robot joints." + ) + + try: + # RDK Initialization + # ========================================================================================== + # Instantiate robot interface + robot = flexivrdk.Robot(args.robot_sn) + + # Clear fault on the connected robot if any + if robot.fault(): + logger.warn("Fault occurred on the connected robot, trying to clear ...") + # Try to clear the fault + if not robot.ClearFault(): + logger.error("Fault cannot be cleared, exiting ...") + return 1 + logger.info("Fault on the connected robot is cleared") + + # Enable the robot, make sure the E-stop is released before enabling + logger.info("Enabling robot ...") + robot.Enable() + + # Wait for the robot to become operational + while not robot.operational(): + time.sleep(1) + + logger.info("Robot is now operational") + + # Move robot to home pose + logger.info("Moving to home pose") + robot.SwitchMode(mode.NRT_PRIMITIVE_EXECUTION) + robot.ExecutePrimitive("Home()") + + # Wait for the primitive to finish + while robot.busy(): + time.sleep(1) + + # Non-real-time Joint Impedance Control + # ========================================================================================== + # Switch to non-real-time joint impedance control mode + robot.SwitchMode(mode.NRT_JOINT_IMPEDANCE) + + period = 1.0 / frequency + loop_counter = 0 + logger.info( + f"Sending command to robot at {frequency} Hz, or {period} seconds interval" + ) + + # Use current robot joint positions as initial positions + init_pos = robot.states().q.copy() + logger.info(f"Initial positions set to: {init_pos}") + + # Robot joint degrees of freedom + DoF = robot.info().DoF + + # Initialize target vectors + target_pos = init_pos.copy() + target_vel = [0.0] * DoF + target_acc = [0.0] * DoF + + # Joint motion constraints + MAX_VEL = [2.0] * DoF + MAX_ACC = [3.0] * DoF + + # Joint sine-sweep amplitude [rad] + SWING_AMP = 0.1 + + # TCP sine-sweep frequency [Hz] + SWING_FREQ = 0.3 + + # Send command periodically at user-specified frequency + while True: + # Use sleep to control loop period + time.sleep(period) + + # Monitor fault on the connected robot + if robot.fault(): + raise Exception("Fault occurred on the connected robot, exiting ...") + + # Sine-sweep all joints + if not args.hold: + for i in range(DoF): + target_pos[i] = init_pos[i] + SWING_AMP * math.sin( + 2 * math.pi * SWING_FREQ * loop_counter * period + ) + # Otherwise all joints will hold at initial positions + + # Reduce joint stiffness to half of nominal values after 5 seconds + if loop_counter == 5 / period: + new_Kq = np.multiply(robot.info().K_q_nom, 0.5) + robot.SetJointImpedance(new_Kq) + logger.info(f"Joint stiffness set to {new_Kq}") + + # Reset joint stiffness to nominal values after another 5 seconds + if loop_counter == 10 / period: + robot.ResetJointImpedance() + logger.info(f"Joint stiffness reset to {robot.info().K_q_nom}") + + # Send commands + robot.SendJointPosition( + target_pos, target_vel, target_acc, MAX_VEL, MAX_ACC + ) + + # Increment loop counter + loop_counter += 1 + + except Exception as e: + # Print exception error message + logger.error(str(e)) + + +if __name__ == "__main__": + main() diff --git a/example_py/intermediate2_non_realtime_cartesian_pure_motion_control.py b/example_py/intermediate3_non_realtime_cartesian_pure_motion_control.py similarity index 77% rename from example_py/intermediate2_non_realtime_cartesian_pure_motion_control.py rename to example_py/intermediate3_non_realtime_cartesian_pure_motion_control.py index 286f1c79..ca7a41fe 100644 --- a/example_py/intermediate2_non_realtime_cartesian_pure_motion_control.py +++ b/example_py/intermediate3_non_realtime_cartesian_pure_motion_control.py @@ -1,25 +1,22 @@ #!/usr/bin/env python -"""intermediate2_non_realtime_cartesian_pure_motion_control.py +"""intermediate3_non_realtime_cartesian_pure_motion_control.py This tutorial runs non-real-time Cartesian-space pure motion control to hold or sine-sweep the robot TCP. A simple collision detection is also included. """ -__copyright__ = "Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved." +__copyright__ = "Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved." __author__ = "Flexiv" import time import math import argparse -import numpy as np +import spdlog # pip install spdlog +import numpy as np # pip install numpy -# Import Flexiv RDK Python library -# fmt: off -import sys -sys.path.insert(0, "../lib_py") +# Flexiv RDK Python library is installed to user site packages import flexivrdk -# fmt: on # Global constants # ================================================================================================== @@ -36,18 +33,6 @@ EXT_TORQUE_THRESHOLD = 5.0 -def print_description(): - """ - Print tutorial description. - - """ - print( - "This tutorial runs non-real-time Cartesian-space pure motion control to hold or " - "sine-sweep the robot TCP. A simple collision detection is also included." - ) - print() - - def main(): # Program Setup # ============================================================================================== @@ -79,23 +64,26 @@ def main(): assert frequency >= 1 and frequency <= 100, "Invalid input" # Define alias - log = flexivrdk.Log() + logger = spdlog.ConsoleLogger("Example") mode = flexivrdk.Mode # Print description - log.Info("Tutorial description:") - print_description() + logger.info( + ">>> Tutorial description <<<\nThis tutorial runs non-real-time Cartesian-space pure " + "motion control to hold or sine-sweep the robot TCP. A simple collision detection is also " + "included." + ) # Print based on arguments if args.hold: - log.Info("Robot holding current TCP pose") + logger.info("Robot holding current TCP pose") else: - log.Info("Robot running TCP sine-sweep") + logger.info("Robot running TCP sine-sweep") if args.collision: - log.Info("Collision detection enabled") + logger.info("Collision detection enabled") else: - log.Info("Collision detection disabled") + logger.info("Collision detection disabled") try: # RDK Initialization @@ -105,25 +93,25 @@ def main(): # Clear fault on the connected robot if any if robot.fault(): - log.Warn("Fault occurred on the connected robot, trying to clear ...") + logger.warn("Fault occurred on the connected robot, trying to clear ...") # Try to clear the fault if not robot.ClearFault(): - log.Error("Fault cannot be cleared, exiting ...") + logger.error("Fault cannot be cleared, exiting ...") return 1 - log.Info("Fault on the connected robot is cleared") + logger.info("Fault on the connected robot is cleared") # Enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ...") + logger.info("Enabling robot ...") robot.Enable() # Wait for the robot to become operational while not robot.operational(): time.sleep(1) - log.Info("Robot is now operational") + logger.info("Robot is now operational") # Move robot to home pose - log.Info("Moving to home pose") + logger.info("Moving to home pose") robot.SwitchMode(mode.NRT_PRIMITIVE_EXECUTION) robot.ExecutePrimitive("Home()") @@ -138,14 +126,14 @@ def main(): # WARNING: during the process, the robot must not contact anything, otherwise the result # will be inaccurate and affect following operations - log.Warn( + logger.warn( "Zeroing force/torque sensors, make sure nothing is in contact with the robot" ) # Wait for primitive completion while robot.busy(): time.sleep(1) - log.Info("Sensor zeroing complete") + logger.info("Sensor zeroing complete") # Configure Motion Control # ========================================================================================= @@ -162,9 +150,8 @@ def main(): # Set initial pose to current TCP pose init_pose = robot.states().tcp_pose.copy() - print( - "Initial TCP pose set to [position 3x1, rotation (quaternion) 4x1]: ", - init_pose, + logger.info( + f"Initial TCP pose set to {init_pose} (position 3x1, rotation (quaternion) 4x1)" ) # Periodic Task @@ -172,12 +159,8 @@ def main(): # Set loop period period = 1.0 / frequency loop_counter = 0 - print( - "Sending command to robot at", - frequency, - "Hz, or", - period, - "seconds interval", + logger.info( + f"Sending command to robot at {frequency} Hz, or {period} seconds interval" ) # Send command periodically at user-specified frequency @@ -209,38 +192,34 @@ def main(): if time_elapsed % 20.0 == 3.0: preferred_jnt_pos = [0.938, -1.108, -1.254, 1.464, 1.073, 0.278, -0.658] robot.SetNullSpacePosture(preferred_jnt_pos) - log.Info("Preferred joint positions set to: ") - print(preferred_jnt_pos) + logger.info(f"Preferred joint positions set to {preferred_jnt_pos}") # Online change stiffness to half of nominal at 6 seconds elif time_elapsed % 20.0 == 6.0: - new_K = np.multiply(robot.info().nominal_K, 0.5) - robot.SetCartesianStiffness(new_K) - log.Info("Cartesian stiffness set to: ") - print(new_K) + new_K = np.multiply(robot.info().K_x_nom, 0.5) + robot.SetCartesianImpedance(new_K) + logger.info(f"Cartesian stiffness set to {new_K}") # Online change to another preferred joint positions at 9 seconds elif time_elapsed % 20.0 == 9.0: preferred_jnt_pos = [-0.938, -1.108, 1.254, 1.464, -1.073, 0.278, 0.658] robot.SetNullSpacePosture(preferred_jnt_pos) - log.Info("Preferred joint positions set to: ") - print(preferred_jnt_pos) + logger.info(f"Preferred joint positions set to {preferred_jnt_pos}") # Online reset stiffness to nominal at 12 seconds elif time_elapsed % 20.0 == 12.0: - robot.ResetCartesianStiffness() - log.Info("Cartesian stiffness is reset") + robot.ResetCartesianImpedance() + logger.info("Cartesian stiffness is reset") # Online reset preferred joint positions to nominal at 14 seconds elif time_elapsed % 20.0 == 14.0: robot.ResetNullSpacePosture() - log.Info("Preferred joint positions are reset") + logger.info("Preferred joint positions are reset") # Online enable max contact wrench regulation at 16 seconds elif time_elapsed % 20.0 == 16.0: max_wrench = [10.0, 10.0, 10.0, 2.0, 2.0, 2.0] robot.SetMaxContactWrench(max_wrench) - log.Info("Max contact wrench set to: ") - print(max_wrench) + logger.info(f"Max contact wrench set to {max_wrench}") # Disable max contact wrench regulation at 19 seconds elif time_elapsed % 20.0 == 19.0: robot.ResetMaxContactWrench() - log.Info("Max contact wrench is reset") + logger.info("Max contact wrench is reset") # Simple collision detection: stop robot if collision is detected at # end-effector @@ -262,7 +241,9 @@ def main(): if collision_detected: robot.Stop() - log.Warn("Collision detected, stopping robot and exit program ...") + logger.warn( + "Collision detected, stopping robot and exit program ..." + ) return # Increment loop counter @@ -270,7 +251,7 @@ def main(): except Exception as e: # Print exception error message - log.Error(str(e)) + logger.error(str(e)) if __name__ == "__main__": diff --git a/example_py/intermediate3_non_realtime_cartesian_motion_force_control.py b/example_py/intermediate4_non_realtime_cartesian_motion_force_control.py similarity index 82% rename from example_py/intermediate3_non_realtime_cartesian_motion_force_control.py rename to example_py/intermediate4_non_realtime_cartesian_motion_force_control.py index 0fbd9d7e..5b338d01 100644 --- a/example_py/intermediate3_non_realtime_cartesian_motion_force_control.py +++ b/example_py/intermediate4_non_realtime_cartesian_motion_force_control.py @@ -1,26 +1,23 @@ #!/usr/bin/env python -"""intermediate3_non_realtime_cartesian_motion_force_control.py +"""intermediate4_non_realtime_cartesian_motion_force_control.py -This tutorial runs real-time Cartesian-space unified motion-force control. The Z axis of the +This tutorial runs non-real-time Cartesian-space unified motion-force control. The Z axis of the chosen reference frame will be activated for explicit force control, while the rest axes in the same reference frame will stay motion controlled. """ -__copyright__ = "Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved." +__copyright__ = "Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved." __author__ = "Flexiv" import time import math import argparse -import numpy as np +import spdlog # pip install spdlog +import numpy as np # pip install numpy -# Import Flexiv RDK Python library -# fmt: off -import sys -sys.path.insert(0, "../lib_py") +# Flexiv RDK Python library is installed to user site packages import flexivrdk -# fmt: on # Global constants # ================================================================================================== @@ -43,19 +40,6 @@ MAX_WRENCH_FOR_CONTACT_SEARCH = [10.0, 10.0, 10.0, 3.0, 3.0, 3.0] -def print_description(): - """ - Print tutorial description. - - """ - print( - "This tutorial runs real-time Cartesian-space unified motion-force control. The Z " - "axis of the chosen reference frame will be activated for explicit force control, " - "while the rest axes in the same reference frame will stay motion controlled." - ) - print() - - def main(): # Program Setup # ============================================================================================== @@ -87,26 +71,32 @@ def main(): assert frequency >= 1 and frequency <= 100, "Invalid input" # Define alias - log = flexivrdk.Log() + logger = spdlog.ConsoleLogger("Example") mode = flexivrdk.Mode # Print description - log.Info("Tutorial description:") - print_description() + logger.info( + ">>> Tutorial description <<<\nThis tutorial runs non-real-time Cartesian-space unified " + "motion-force control. The Z axis of the chosen reference frame will be activated for " + "explicit force control, while the rest axes in the same reference frame will stay motion " + "controlled." + ) # The reference frame to use, see Robot::SendCartesianMotionForce() for more details frame_str = "WORLD" if args.TCP: - log.Info("Reference frame used for force control: robot TCP frame") + logger.info("Reference frame used for force control: robot TCP frame") frame_str = "TCP" else: - log.Info("Reference frame used for force control: robot world frame") + logger.info("Reference frame used for force control: robot world frame") # Whether to enable polish motion if args.polish: - log.Info("Robot will run a polish motion along XY plane in robot world frame") + logger.info( + "Robot will run a polish motion along XY plane in robot world frame" + ) else: - log.Info("Robot will hold its motion in all non-force-controlled axes") + logger.info("Robot will hold its motion in all non-force-controlled axes") try: # RDK Initialization @@ -116,25 +106,25 @@ def main(): # Clear fault on the connected robot if any if robot.fault(): - log.Warn("Fault occurred on the connected robot, trying to clear ...") + logger.warn("Fault occurred on the connected robot, trying to clear ...") # Try to clear the fault if not robot.ClearFault(): - log.Error("Fault cannot be cleared, exiting ...") + logger.error("Fault cannot be cleared, exiting ...") return 1 - log.Info("Fault on the connected robot is cleared") + logger.info("Fault on the connected robot is cleared") # Enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ...") + logger.info("Enabling robot ...") robot.Enable() # Wait for the robot to become operational while not robot.operational(): time.sleep(1) - log.Info("Robot is now operational") + logger.info("Robot is now operational") # Move robot to home pose - log.Info("Moving to home pose") + logger.info("Moving to home pose") robot.SwitchMode(mode.NRT_PRIMITIVE_EXECUTION) robot.ExecutePrimitive("Home()") @@ -149,27 +139,26 @@ def main(): # WARNING: during the process, the robot must not contact anything, otherwise the result # will be inaccurate and affect following operations - log.Warn( + logger.warn( "Zeroing force/torque sensors, make sure nothing is in contact with the robot" ) # Wait for primitive completion while robot.busy(): time.sleep(1) - log.Info("Sensor zeroing complete") + logger.info("Sensor zeroing complete") # Search for Contact # ========================================================================================= # NOTE: there are several ways to do contact search, such as using primitives, or real-time # and non-real-time direct motion controls, etc. Here we use non-real-time direct Cartesian # control for example. - log.Info("Searching for contact ...") + logger.info("Searching for contact ...") # Set initial pose to current TCP pose init_pose = robot.states().tcp_pose.copy() - print( - "Initial TCP pose set to [position 3x1, rotation (quaternion) 4x1]: ", - init_pose, + logger.info( + f"Initial TCP pose set to {init_pose} (position 3x1, rotation (quaternion) 4x1)" ) # Use non-real-time mode to make the robot go to a set point with its own motion generator @@ -201,7 +190,7 @@ def main(): # Contact is considered to be made if sensed TCP force exceeds the threshold if np.linalg.norm(ext_force) > PRESSING_FORCE: is_contacted = True - log.Info("Contact detected at robot TCP") + logger.info("Contact detected at robot TCP") # Check at 1ms interval time.sleep(0.001) @@ -239,21 +228,16 @@ def main(): # Update initial pose to current TCP pose init_pose = robot.states().tcp_pose.copy() - print( - "Initial TCP pose set to [position 3x1, rotation (quaternion) 4x1]: ", - init_pose, + logger.info( + f"Initial TCP pose set to {init_pose} (position 3x1, rotation (quaternion) 4x1)" ) # Periodic Task # ========================================================================================= # Set loop period period = 1.0 / frequency - print( - "Sending command to robot at", - frequency, - "Hz, or", - period, - "seconds interval", + logger.info( + f"Sending command to robot at {frequency} Hz, or {period} seconds interval" ) # Periodic loop counter @@ -299,7 +283,7 @@ def main(): except Exception as e: # Print exception error message - log.Error(str(e)) + logger.error(str(e)) if __name__ == "__main__": diff --git a/example_py/intermediate5_robot_dynamics.py b/example_py/intermediate5_robot_dynamics.py new file mode 100644 index 00000000..6d44b79a --- /dev/null +++ b/example_py/intermediate5_robot_dynamics.py @@ -0,0 +1,116 @@ +#!/usr/bin/env python + +"""intermediate5_robot_dynamics.py + +This tutorial runs the integrated dynamics engine to obtain robot Jacobian, mass matrix, and +gravity torques. Also checks reachability of a Cartesian pose. +""" + +__copyright__ = "Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved." +__author__ = "Flexiv" + +import time +import argparse +import spdlog # pip install spdlog + +# Flexiv RDK Python library is installed to user site packages +import flexivrdk + + +def main(): + # Program Setup + # ============================================================================================== + # Parse arguments + argparser = argparse.ArgumentParser() + argparser.add_argument( + "robot_sn", + help="Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456", + ) + args = argparser.parse_args() + + # Define alias + logger = spdlog.ConsoleLogger("Example") + mode = flexivrdk.Mode + + # Print description + logger.info( + ">>> Tutorial description <<<\nThis tutorial runs the integrated dynamics engine to obtain " + "robot Jacobian, mass matrix, and gravity torques. Also checks reachability of a Cartesian " + "pose." + ) + + try: + # RDK Initialization + # ========================================================================================== + # Instantiate robot interface + robot = flexivrdk.Robot(args.robot_sn) + + # Clear fault on the connected robot if any + if robot.fault(): + logger.warn("Fault occurred on the connected robot, trying to clear ...") + # Try to clear the fault + if not robot.ClearFault(): + logger.error("Fault cannot be cleared, exiting ...") + return 1 + logger.info("Fault on the connected robot is cleared") + + # Enable the robot, make sure the E-stop is released before enabling + logger.info("Enabling robot ...") + robot.Enable() + + # Wait for the robot to become operational + while not robot.operational(): + time.sleep(1) + + logger.info("Robot is now operational") + + # Move robot to home pose + logger.info("Moving to home pose") + robot.SwitchMode(mode.NRT_PRIMITIVE_EXECUTION) + robot.ExecutePrimitive("Home()") + + # Wait for the primitive to finish + while robot.busy(): + time.sleep(1) + + # Robot Dynamics + # ========================================================================================== + # Initialize dynamics engine + model = flexivrdk.Model(robot) + + # Step dynamics engine 5 times + for i in range(5): + # Update robot model in dynamics engine + model.Update(robot.states().q, robot.states().dtheta) + + # Compute gravity vector + g = model.g() + + # Compute mass matrix + M = model.M() + + # Compute Jacobian + J = model.J("flange") + + # Print result + logger.info("g = ") + print(g, flush=True) + logger.info("M = ") + print(M, flush=True) + logger.info("J = ") + print(J, flush=True) + print() + + # Check reachability of a Cartesian pose based on current pose + pose_to_check = robot.states().tcp_pose.copy() + pose_to_check[0] += 0.1 + logger.info(f"Checking reachability of Cartesian pose {pose_to_check}") + result = model.reachable(pose_to_check, robot.states().q, True) + logger.info(f"Got a result: reachable = {result[0]}, IK solution = {result[1]}") + + except Exception as e: + logger.error(str(e)) + + +if __name__ == "__main__": + main() diff --git a/example_py/utility.py b/example_py/utility.py index 321125b4..88169644 100644 --- a/example_py/utility.py +++ b/example_py/utility.py @@ -5,7 +5,7 @@ Utility methods. """ -__copyright__ = "Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved." +__copyright__ = "Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved." __author__ = "Flexiv" import math diff --git a/include/flexiv/log.h b/include/flexiv/log.h deleted file mode 100644 index 98955879..00000000 --- a/include/flexiv/log.h +++ /dev/null @@ -1,50 +0,0 @@ -/** - * @file log.h - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. - */ - -#ifndef FLEXIVRDK_LOG_H_ -#define FLEXIVRDK_LOG_H_ - -#include - -namespace flexiv { - -/** - * @class Log - * @brief Helper functions to print messages with timestamp and coloring. - * @warning For maximum portability, std::cout is used to handle the printing, which is a relatively - * slow process compared to dedicated performance loggers. Therefore, it's not recommended to use - * flexiv::Log in performance-critical loops. - */ -class Log -{ -public: - Log() = default; - virtual ~Log() = default; - - /** - * @brief [Non-blocking] Print info message with timestamp and coloring. - * @param[in] message Info message. - * @note Color = green. - */ - void Info(const std::string& message) const; - - /** - * @brief [Non-blocking] Print warning message with timestamp and coloring. - * @param[in] message Warning message. - * @note Color = yellow. - */ - void Warn(const std::string& message) const; - - /** - * @brief [Non-blocking] Print error message with timestamp and coloring. - * @param[in] message Error message. - * @note Color = red. - */ - void Error(const std::string& message) const; -}; - -} /* namespace flexiv */ - -#endif /* FLEXIVRDK_LOG_H_ */ diff --git a/include/flexiv/mode.h b/include/flexiv/mode.h deleted file mode 100644 index b902a4b6..00000000 --- a/include/flexiv/mode.h +++ /dev/null @@ -1,89 +0,0 @@ -/** - * @file mode.h - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. - */ - -#ifndef FLEXIVRDK_MODE_H_ -#define FLEXIVRDK_MODE_H_ - -namespace flexiv { - -/** - * @enum Mode - * @brief Robot control modes. Must put the robot into the correct control mode before sending any - * corresponding commands. - */ -enum Mode -{ - /** Mode not set */ - UNKNOWN = 0, - - /** - * No operation to execute, the robot holds position and waits for new command. - */ - IDLE, - - /** - * Run real-time joint torque control to track continuous commands @1kHz. - * @note Real-time (RT) mode - * @see flexiv::Robot::StreamJointTorque() - */ - RT_JOINT_TORQUE, - - /** - * Run real-time joint position control to track continuous commands @ 1kHz. - * @note Real-time (RT) mode - * @see flexiv::Robot::StreamJointPosition() - */ - RT_JOINT_POSITION, - - /** - * Run non-real-time joint position control to track discrete commands (smoothened by internal - * motion generator). - * @note Non-real-time (NRT) mode - * @see flexiv::Robot::SendJointPosition() - */ - NRT_JOINT_POSITION, - - /** - * Execute pre-configured robot task plans. - * @note Non-real-time (NRT) mode - * @see flexiv::Robot::ExecutePlan() - */ - NRT_PLAN_EXECUTION, - - /** - * Execute robot primitives (unit skills). - * @note Non-real-time (NRT) mode - * @see flexiv::Robot::ExecutePrimitive() - * @see [Flexiv Primitives](https://www.flexiv.com/primitives/) documentation - */ - NRT_PRIMITIVE_EXECUTION, - - /** - * Run real-time Cartesian motion-force control to track continuous commands @ 1kHz. - * @note Real-time (RT) mode - * @see flexiv::Robot::StreamCartesianMotionForce() - */ - RT_CARTESIAN_MOTION_FORCE, - - /** - * Run non-real-time Cartesian motion-force control to track discrete commands, smoothened by - * internal motion generator. - * @note Non-real-time (NRT) mode - * @see flexiv::Robot::SendCartesianMotionForce() - */ - NRT_CARTESIAN_MOTION_FORCE, - - /** Total number of control modes */ - MODES_CNT, -}; - -/** String names of the above control modes */ -static const std::array kModeNames = {"UNKNOWN", "IDLE", "RT_JOINT_TORQUE", - "RT_JOINT_POSITION", "NRT_JOINT_POSITION", "NRT_PLAN_EXECUTION", "NRT_PRIMITIVE_EXECUTION", - "RT_CARTESIAN_MOTION_FORCE", "NRT_CARTESIAN_MOTION_FORCE"}; - -} /* namespace flexiv */ - -#endif /* FLEXIVRDK_MODE_H_ */ diff --git a/include/flexiv/data.h b/include/flexiv/rdk/data.hpp similarity index 77% rename from include/flexiv/data.h rename to include/flexiv/rdk/data.hpp index 90f80e13..e6427a0f 100644 --- a/include/flexiv/data.h +++ b/include/flexiv/rdk/data.hpp @@ -1,23 +1,22 @@ /** - * @file data.h + * @file data.hpp * @brief Header file containing various data structs. - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. */ -#ifndef FLEXIVRDK_DATA_H_ -#define FLEXIVRDK_DATA_H_ +#ifndef FLEXIV_RDK_DATA_HPP_ +#define FLEXIV_RDK_DATA_HPP_ #include +#include #include #include namespace flexiv { - -/** Robot joint-space degrees of freedom \f$ n \f$ */ -constexpr size_t kJointDOF = 7; +namespace rdk { /** Robot Cartesian-space degrees of freedom \f$ m \f$ */ -constexpr size_t kCartDOF = 6; +constexpr size_t kCartDoF = 6; /** Size of pose array (3 position + 4 quaternion) */ constexpr size_t kPoseSize = 7; @@ -27,52 +26,64 @@ constexpr size_t kIOPorts = 16; /** * @struct RobotInfo - * @brief General information of the connected robot. + * @brief General information about the connected robot. */ struct RobotInfo { - /** Robot serial number */ + /** Robot serial number. */ std::string serial_num = {}; - /** Robot software version */ + /** Robot software version. */ std::string software_ver = {}; + /** Robot model name, e.g. Rizon4, Rizon10, Moonlight, etc. */ + std::string model_name = {}; + + /** Joint-space degrees of freedom: \f$ n \f$. */ + size_t DoF = {}; + /** - * Nominal motion stiffness of the Cartesian motion-force control modes: \f$ K_{nom} \in + * Nominal motion stiffness of the Cartesian motion-force control modes: \f$ K_x^{nom} \in * \mathbb{R}^{6 \times 1} \f$. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ linear stiffness and * \f$ \mathbb{R}^{3 \times 1} \f$ angular stiffness: \f$ [k_x, k_y, k_z, k_{Rx}, k_{Ry}, * k_{Rz}]^T \f$. Unit: \f$ [N/m]~[Nm/rad] \f$. */ - std::array nominal_K = {}; + std::array K_x_nom = {}; + + /** + * Nominal motion stiffness of the joint impedance control modes: \f$ K_q^{nom} \in + * \mathbb{R}^{n \times 1} \f$. Unit: \f$ [Nm/rad] \f$. + */ + std::vector K_q_nom = {}; /** * Lower limits of joint positions: \f$ q_{min} \in \mathbb{R}^{n \times 1} \f$. * Unit: \f$ [rad] \f$. */ - std::array q_min = {}; + std::vector q_min = {}; /** * Upper limits of joint positions: \f$ q_{max} \in \mathbb{R}^{n \times 1} \f$. * Unit: \f$ [rad] \f$. */ - std::array q_max = {}; + std::vector q_max = {}; /** * Upper limits of joint velocities: \f$ \dot{q}_{max} \in \mathbb{R}^{n \times 1} \f$. * Unit: \f$ [rad/s] \f$. */ - std::array dq_max = {}; + std::vector dq_max = {}; /** * Upper limits of joint torques: \f$ \tau_{max} \in \mathbb{R}^{n \times 1} \f$. * Unit: \f$ [Nm] \f$. */ - std::array tau_max = {}; + std::vector tau_max = {}; }; /** * @struct RobotStates - * @brief Data struct containing the joint- and Cartesian-space robot states. + * @brief Data structure containing the joint- and Cartesian-space robot states. */ struct RobotStates { @@ -81,7 +92,7 @@ struct RobotStates * This is the direct measurement of joint positions, preferred for most cases. * Unit: \f$ [rad] \f$. */ - std::array q = {}; + std::vector q = {}; /** * Measured joint positions using motor-side encoder: \f$ \theta \in \mathbb{R}^{n \times 1} @@ -89,45 +100,45 @@ struct RobotStates * \f$ \Delta \f$ is the joint's internal deflection between motor and link. * Unit: \f$ [rad] \f$. */ - std::array theta = {}; + std::vector theta = {}; /** * Measured joint velocities using link-side encoder: \f$ \dot{q} \in \mathbb{R}^{n \times 1} * \f$. This is the direct but more noisy measurement of joint velocities. * Unit: \f$ [rad/s] \f$. */ - std::array dq = {}; + std::vector dq = {}; /** * Measured joint velocities using motor-side encoder: \f$ \dot{\theta} \in \mathbb{R}^{n \times * 1} \f$. This is the indirect but less noisy measurement of joint velocities, preferred for * most cases. Unit: \f$ [rad/s] \f$. */ - std::array dtheta = {}; + std::vector dtheta = {}; /** * Measured joint torques: \f$ \tau \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [Nm] \f$. */ - std::array tau = {}; + std::vector tau = {}; /** * Desired joint torques: \f$ \tau_{d} \in \mathbb{R}^{n \times 1} \f$. Compensation of * nonlinear dynamics (gravity, centrifugal, and Coriolis) is excluded. Unit: \f$ [Nm] \f$. */ - std::array tau_des = {}; + std::vector tau_des = {}; /** * Numerical derivative of measured joint torques: \f$ \dot{\tau} \in \mathbb{R}^{n \times 1} * \f$. Unit: \f$ [Nm/s] \f$. */ - std::array tau_dot = {}; + std::vector tau_dot = {}; /** * Estimated external joint torques: \f$ \hat \tau_{ext} \in \mathbb{R}^{n \times 1} \f$. * Produced by any external contact (with robot body or end-effector) that does not belong to * the known robot model. Unit: \f$ [Nm] \f$. */ - std::array tau_ext = {}; + std::vector tau_ext = {}; /** * Measured TCP pose expressed in world frame: \f$ ^{O}T_{TCP} \in \mathbb{R}^{7 \times 1} \f$. @@ -149,7 +160,7 @@ struct RobotStates * 1} \f$ angular velocity: \f$ [v_x, v_y, v_z, \omega_x, \omega_y, \omega_z]^T \f$. * Unit: \f$ [m/s]~[rad/s] \f$. */ - std::array tcp_vel = {}; + std::array tcp_vel = {}; /** * Measured flange pose expressed in world frame: \f$ ^{O}T_{flange} \in \mathbb{R}^{7 \times 1} @@ -164,7 +175,7 @@ struct RobotStates * \times 1} \f$ force and \f$ \mathbb{R}^{3 \times 1} \f$ moment: \f$ [f_x, f_y, f_z, m_x, m_y, * m_z]^T \f$. Unit: \f$ [N]~[Nm] \f$. */ - std::array ft_sensor_raw = {}; + std::array ft_sensor_raw = {}; /** * Estimated external wrench applied on TCP and expressed in TCP frame: \f$ ^{TCP}F_{ext} \in @@ -172,7 +183,7 @@ struct RobotStates * \mathbb{R}^{3 \times 1} \f$ moment: \f$ [f_x, f_y, f_z, m_x, m_y, m_z]^T \f$. * Unit: \f$ [N]~[Nm] \f$. */ - std::array ext_wrench_in_tcp = {}; + std::array ext_wrench_in_tcp = {}; /** * Estimated external wrench applied on TCP and expressed in world frame: \f$ ^{0}F_{ext} \in @@ -180,12 +191,12 @@ struct RobotStates * \mathbb{R}^{3 \times 1} \f$ moment: \f$ [f_x, f_y, f_z, m_x, m_y, m_z]^T \f$. * Unit: \f$ [N]~[Nm] \f$. */ - std::array ext_wrench_in_world = {}; + std::array ext_wrench_in_world = {}; }; /** * @struct PlanInfo - * @brief Data struct containing information of the on-going primitive/plan. + * @brief Data structure containing information of the on-going primitive/plan. */ struct PlanInfo { @@ -216,7 +227,7 @@ struct PlanInfo /** * @struct GripperStates - * @brief Data struct containing the gripper states. + * @brief Data structure containing the gripper states. */ struct GripperStates { @@ -233,7 +244,7 @@ struct GripperStates /** * @struct ToolParams - * @brief Data struct containing robot tool parameters. + * @brief Data structure containing robot tool parameters. */ struct ToolParams { @@ -256,38 +267,39 @@ struct ToolParams * @brief Operator overloading to out stream all robot info in JSON format: * {"info_1": [val1,val2,val3,...], "info_2": [val1,val2,val3,...], ...}. * @param[in] ostream Ostream instance. - * @param[in] robot_info RobotInfo data struct to out stream. + * @param[in] robot_info RobotInfo data structure to out stream. * @return Updated ostream instance. */ -std::ostream& operator<<(std::ostream& ostream, const flexiv::RobotInfo& robot_info); +std::ostream& operator<<(std::ostream& ostream, const RobotInfo& robot_info); /** * @brief Operator overloading to out stream all robot states in JSON format: * {"state_1": [val1,val2,val3,...], "state_2": [val1,val2,val3,...], ...}. * @param[in] ostream Ostream instance. - * @param[in] robot_states RobotStates data struct to out stream. + * @param[in] robot_states RobotStates data structure to out stream. * @return Updated ostream instance. */ -std::ostream& operator<<(std::ostream& ostream, const flexiv::RobotStates& robot_states); +std::ostream& operator<<(std::ostream& ostream, const RobotStates& robot_states); /** * @brief Operator overloading to out stream all plan info in JSON format: * {"info_1": [val1,val2,val3,...], "info_2": [val1,val2,val3,...], ...}. * @param[in] ostream Ostream instance. - * @param[in] plan_info PlanInfo data struct to out stream. + * @param[in] plan_info PlanInfo data structure to out stream. * @return Updated ostream instance. */ -std::ostream& operator<<(std::ostream& ostream, const flexiv::PlanInfo& plan_info); +std::ostream& operator<<(std::ostream& ostream, const PlanInfo& plan_info); /** * @brief Operator overloading to out stream all gripper states in JSON format: * {"state_1": [val1,val2,val3,...], "state_2": [val1,val2,val3,...], ...}. * @param[in] ostream Ostream instance. - * @param[in] gripper_states GripperStates data struct to out stream. + * @param[in] gripper_states GripperStates data structure to out stream. * @return Updated ostream instance. */ -std::ostream& operator<<(std::ostream& ostream, const flexiv::GripperStates& gripper_states); +std::ostream& operator<<(std::ostream& ostream, const GripperStates& gripper_states); +} /* namespace rdk */ } /* namespace flexiv */ -#endif /* FLEXIVRDK_DATA_H_ */ +#endif /* FLEXIV_RDK_DATA_HPP_ */ diff --git a/include/flexiv/device.h b/include/flexiv/rdk/device.hpp similarity index 87% rename from include/flexiv/device.h rename to include/flexiv/rdk/device.hpp index 9cc7ff93..db0721cc 100644 --- a/include/flexiv/device.h +++ b/include/flexiv/rdk/device.hpp @@ -1,15 +1,16 @@ /** - * @file device.h - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * @file device.hpp + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. */ -#ifndef FLEXIVRDK_DEVICE_H_ -#define FLEXIVRDK_DEVICE_H_ +#ifndef FLEXIV_RDK_DEVICE_HPP_ +#define FLEXIV_RDK_DEVICE_HPP_ -#include "robot.h" +#include "robot.hpp" #include namespace flexiv { +namespace rdk { /** * @class Device @@ -20,7 +21,7 @@ class Device public: /** * @brief [Non-blocking] Create an instance and initialize device control interface. - * @param[in] robot Reference to the instance of flexiv::Robot. + * @param[in] robot Reference to the instance of flexiv::rdk::Robot. * @throw std::runtime_error if the initialization sequence failed. */ Device(const Robot& robot); @@ -33,7 +34,7 @@ class Device * @throw std::runtime_error if failed to get a reply from the connected robot. * @note This function blocks until a reply is received. */ - std::map list() const; + const std::map list() const; /** * @brief [Blocking] Enable the specified device. @@ -69,6 +70,7 @@ class Device std::unique_ptr pimpl_; }; +} /* namespace rdk */ } /* namespace flexiv */ -#endif /* FLEXIVRDK_DEVICE_H_ */ +#endif /* FLEXIV_RDK_DEVICE_HPP_ */ diff --git a/include/flexiv/file_io.h b/include/flexiv/rdk/file_io.hpp similarity index 74% rename from include/flexiv/file_io.h rename to include/flexiv/rdk/file_io.hpp index 6af2eab2..71b6baf3 100644 --- a/include/flexiv/file_io.h +++ b/include/flexiv/rdk/file_io.hpp @@ -1,26 +1,27 @@ /** - * @file file_io.h - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * @file file_io.hpp + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. */ -#ifndef FLEXIVRDK_FILE_IO_H_ -#define FLEXIVRDK_FILE_IO_H_ +#ifndef FLEXIV_RDK_FILE_IO_HPP_ +#define FLEXIV_RDK_FILE_IO_HPP_ -#include "robot.h" +#include "robot.hpp" namespace flexiv { +namespace rdk { /** * @class FileIO - * @brief Interface for doing file transfer with the robot. The robot must be put into IDLE mode - * when transferring files. + * @brief Interface for file transfer with the robot. The robot must be put into IDLE mode when + * transferring files. */ class FileIO { public: /** * @brief [Non-blocking] Create an instance and initialize file transfer interface. - * @param[in] robot Reference to the instance of flexiv::Robot. + * @param[in] robot Reference to the instance of flexiv::rdk::Robot. * @throw std::runtime_error if the initialization sequence failed. */ FileIO(const Robot& robot); @@ -45,6 +46,7 @@ class FileIO std::unique_ptr pimpl_; }; +} /* namespace rdk */ } /* namespace flexiv */ -#endif /* FLEXIVRDK_FILE_IO_H_ */ +#endif /* FLEXIV_RDK_FILE_IO_HPP_ */ diff --git a/include/flexiv/gripper.h b/include/flexiv/rdk/gripper.hpp similarity index 87% rename from include/flexiv/gripper.h rename to include/flexiv/rdk/gripper.hpp index 4c68ecf0..d198682f 100644 --- a/include/flexiv/gripper.h +++ b/include/flexiv/rdk/gripper.hpp @@ -1,15 +1,16 @@ /** - * @file gripper.h - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * @file gripper.hpp + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. */ -#ifndef FLEXIVRDK_GRIPPER_H_ -#define FLEXIVRDK_GRIPPER_H_ +#ifndef FLEXIV_RDK_GRIPPER_HPP_ +#define FLEXIV_RDK_GRIPPER_HPP_ -#include "robot.h" +#include "robot.hpp" #include namespace flexiv { +namespace rdk { /** * @class Gripper @@ -20,7 +21,7 @@ class Gripper public: /** * @brief [Non-blocking] Create an instance and initialize gripper control interface. - * @param[in] robot Reference to the instance of flexiv::Robot. + * @param[in] robot Reference to the instance of flexiv::rdk::Robot. * @throw std::runtime_error if the initialization sequence failed. */ Gripper(const Robot& robot); @@ -75,13 +76,14 @@ class Gripper * @return GripperStates value copy. * @note Real-time (RT). */ - GripperStates states() const; + const GripperStates states() const; private: class Impl; std::unique_ptr pimpl_; }; +} /* namespace rdk */ } /* namespace flexiv */ -#endif /* FLEXIVRDK_GRIPPER_H_ */ +#endif /* FLEXIV_RDK_GRIPPER_HPP_ */ diff --git a/include/flexiv/rdk/mode.hpp b/include/flexiv/rdk/mode.hpp new file mode 100644 index 00000000..fe1582a8 --- /dev/null +++ b/include/flexiv/rdk/mode.hpp @@ -0,0 +1,108 @@ +/** + * @file mode.hpp + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. + */ + +#ifndef FLEXIV_RDK_MODE_HPP_ +#define FLEXIV_RDK_MODE_HPP_ + +namespace flexiv { +namespace rdk { + +/** + * @enum Mode + * @brief Robot control modes. The robot needs to be switched into the correct control mode before + * the corresponding commands can be sent. Refer to Flexiv RDK Manual for more details. + * @see Robot::SwitchMode(). + */ +enum Mode +{ + /** Mode not set */ + UNKNOWN = 0, + + /** + * No operation to execute, the robot holds position and waits for new command. + */ + IDLE, + + /** + * Run real-time joint torque control to track continuous commands @1kHz. + * @note Real-time (RT) mode. + * @see Robot::StreamJointTorque(). + */ + RT_JOINT_TORQUE, + + /** + * Run real-time joint impedance control to track continuous commands @ 1kHz. + * @note Real-time (RT) mode. + * @see Robot::StreamJointPosition(). + */ + RT_JOINT_IMPEDANCE, + + /** + * Run non-real-time joint impedance control to track discrete commands (smoothened by internal + * motion generator). + * @note Non-real-time (NRT) mode. + * @see Robot::SendJointPosition(). + */ + NRT_JOINT_IMPEDANCE, + + /** + * Run real-time joint position control to track continuous commands @ 1kHz. + * @note Real-time (RT) mode. + * @see Robot::StreamJointPosition(). + */ + RT_JOINT_POSITION, + + /** + * Run non-real-time joint position control to track discrete commands (smoothened by internal + * motion generator). + * @note Non-real-time (NRT) mode. + * @see Robot::SendJointPosition(). + */ + NRT_JOINT_POSITION, + + /** + * Execute pre-configured robot task plans. + * @note Non-real-time (NRT) mode. + * @see Robot::ExecutePlan(). + */ + NRT_PLAN_EXECUTION, + + /** + * Execute robot primitives (unit skills). + * @note Non-real-time (NRT) mode. + * @see Robot::ExecutePrimitive(). + * @see [Flexiv Primitives](https://www.flexiv.com/primitives/) documentation. + */ + NRT_PRIMITIVE_EXECUTION, + + /** + * Run real-time Cartesian motion-force control to track continuous commands @ 1kHz. + * @note Real-time (RT) mode. + * @see Robot::StreamCartesianMotionForce(). + */ + RT_CARTESIAN_MOTION_FORCE, + + /** + * Run non-real-time Cartesian motion-force control to track discrete commands, smoothened by + * internal motion generator. + * @note Non-real-time (NRT) mode. + * @see Robot::SendCartesianMotionForce(). + */ + NRT_CARTESIAN_MOTION_FORCE, + + /** Total number of control modes */ + MODES_CNT, +}; + +/** String names of the above control modes */ +static const std::array kModeNames + = {"UNKNOWN", "IDLE", "RT_JOINT_TORQUE", "RT_JOINT_IMPEDANCE", "NRT_JOINT_IMPEDANCE", + "RT_JOINT_POSITION", "NRT_JOINT_POSITION", "NRT_PLAN_EXECUTION", "NRT_PRIMITIVE_EXECUTION", + "RT_CARTESIAN_MOTION_FORCE", "NRT_CARTESIAN_MOTION_FORCE"}; + +} /* namespace rdk */ +} /* namespace flexiv */ + +#endif /* FLEXIV_RDK_MODE_HPP_ */ diff --git a/include/flexiv/model.h b/include/flexiv/rdk/model.hpp similarity index 70% rename from include/flexiv/model.h rename to include/flexiv/rdk/model.hpp index 2846bb40..3377c755 100644 --- a/include/flexiv/model.h +++ b/include/flexiv/rdk/model.hpp @@ -1,27 +1,28 @@ /** - * @file model.h - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * @file model.hpp + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. */ -#ifndef FLEXIVRDK_MODEL_H_ -#define FLEXIVRDK_MODEL_H_ +#ifndef FLEXIV_RDK_MODEL_HPP_ +#define FLEXIV_RDK_MODEL_HPP_ -#include "robot.h" +#include "robot.hpp" #include #include namespace flexiv { +namespace rdk { /** * @class Model - * @brief Robot model with integrated dynamics engine. + * @brief Interface to access certain robot kinematics and dynamics data. */ class Model { public: /** * @brief [Non-blocking] Create an instance and initialize the integrated dynamics engine. - * @param[in] robot Reference to the instance of flexiv::Robot. + * @param[in] robot Reference to the instance of flexiv::rdk::Robot. * @param[in] gravity_vector Earth's gravity vector in world frame. Default to \f$ [0.0, 0.0, * -9.81]^T \f$. Unit: \f$ [m/s^2] \f$. * @throw std::runtime_error if the initialization sequence failed. @@ -48,16 +49,16 @@ class Model * \f$ [rad] \f$. * @param[in] velocities Current joint velocities: \f$ \dot{q} \in \mathbb{R}^{n \times 1} * \f$. Unit: \f$ [rad/s] \f$. + * @throw std::invalid_argument if size of any input vector does not match robot DoF. */ - void Update(const std::array& positions, - const std::array& velocities); + void Update(const std::vector& positions, const std::vector& velocities); /** * @brief [Non-blocking] Compute and get the Jacobian matrix at the frame of the specified link * \f$ i \f$, expressed in world frame. * @param[in] link_name Name of the link to get Jacobian for. * @return Jacobian matrix: \f$ ^{0}J_i \in \mathbb{R}^{m \times n} \f$. - * @note Call update() before this method. + * @note Call Update() before this function. * @note Available links can be found in the provided URDF. They are {"base_link", "link1", * "link2", "link3", "link4", "link5", "link6", "link7", "flange"}, plus "tool" if any flange * tool is mounted. @@ -71,7 +72,7 @@ class Model * @param[in] link_name Name of the link to get Jacobian derivative for. * @return Time derivative of Jacobian matrix: \f$ ^{0}\dot{J_i} \in \mathbb{R}^{m \times n} * \f$. - * @note Call update() before this method. + * @note Call Update() before this function. * @note Available links can be found in the provided URDF. They are {"base_link", "link1", * "link2", "link3", "link4", "link5", "link6", "link7", "flange"}, plus "tool" if any flange * tool is mounted. @@ -84,7 +85,7 @@ class Model * joint space. * @return Symmetric positive definite mass matrix: \f$ M(q) \in \mathbb{S}^{n \times n}_{++} * \f$. Unit: \f$ [kgm^2] \f$. - * @note Call update() before this method. + * @note Call Update() before this function. */ const Eigen::MatrixXd M(); @@ -92,7 +93,7 @@ class Model * @brief [Non-blocking] Compute and get the Coriolis/centripetal matrix for the generalized * coordinates, i.e. joint space. * @return Coriolis/centripetal matrix: \f$ C(q,\dot{q}) \in \mathbb{R}^{n \times n} \f$. - * @note Call update() before this method. + * @note Call Update() before this function. */ const Eigen::MatrixXd C(); @@ -100,7 +101,7 @@ class Model * @brief [Non-blocking] Compute and get the gravity force vector for the generalized * coordinates, i.e. joint space. * @return Gravity force vector: \f$ g(q) \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [Nm] \f$. - * @note Call update() before this method. + * @note Call Update() before this function. */ const Eigen::VectorXd g(); @@ -109,15 +110,31 @@ class Model * coordinates, i.e. joint space. * @return Coriolis force vector: \f$ c(q,\dot{q}) \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ * [Nm] \f$. - * @note Call update() before this method. + * @note Call Update() before this function. */ const Eigen::VectorXd c(); + /** + * @brief [Blocking] Check if a Cartesian pose is reachable. If yes, also return an IK solution + * of the corresponding joint positions. + * @param[in] pose Cartesian pose to be checked. + * @param[in] seed_positions Joint positions to be used as the seed for solving IK. + * @param[in] free_orientation Only constrain position and allow orientation to move freely. + * @return A pair of . + * @throw std::invalid_argument if size of [seed_positions] does not match robot DoF. + * @throw std::runtime_error if failed to get a reply from the connected robot. + * @note Applicable control modes: All. + * @note This function blocks until a reply is received. + */ + const std::pair> reachable(const std::array& pose, + const std::vector& seed_positions, bool free_orientation); + private: class Impl; std::unique_ptr pimpl_; }; +} /* namespace rdk */ } /* namespace flexiv */ -#endif /* FLEXIVRDK_MODEL_H_ */ +#endif /* FLEXIV_RDK_MODEL_HPP_ */ diff --git a/include/flexiv/robot.h b/include/flexiv/rdk/robot.hpp similarity index 80% rename from include/flexiv/robot.h rename to include/flexiv/rdk/robot.hpp index dcf1178d..3ab6f00f 100644 --- a/include/flexiv/robot.h +++ b/include/flexiv/rdk/robot.hpp @@ -1,19 +1,19 @@ /** - * @file robot.h - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * @file robot.hpp + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. */ -#ifndef FLEXIVRDK_ROBOT_H_ -#define FLEXIVRDK_ROBOT_H_ - -#include "data.h" -#include "mode.h" +#ifndef FLEXIV_RDK_ROBOT_HPP_ +#define FLEXIV_RDK_ROBOT_HPP_ +#include "data.hpp" +#include "mode.hpp" #include #include #include namespace flexiv { +namespace rdk { /** * @class Robot @@ -28,6 +28,7 @@ class Robot * initialize and connection with the robot will be established. * @param[in] robot_sn Serial number of the robot to connect. The accepted formats are: * "Rizon 4s-123456" and "Rizon4s-123456". + * @throw std::invalid_argument if the format of [robot_sn] is invalid. * @throw std::runtime_error if the initialization sequence failed. * @throw std::logic_error if the connected robot does not have a valid RDK license; or this RDK * library version is incompatible with the connected robot; or model of the connected robot is @@ -49,11 +50,11 @@ class Robot * @brief [Non-blocking] Access general information of the robot. * @return RobotInfo value copy. */ - RobotInfo info() const; + const RobotInfo info() const; /** * @brief [Non-blocking] Access current control mode of the connected robot. - * @return flexiv::Mode enum. + * @return flexiv::rdk::Mode enum. */ Mode mode() const; @@ -62,7 +63,7 @@ class Robot * @return RobotStates value copy. * @note Real-time (RT). */ - RobotStates states() const; + const RobotStates states() const; /** * @brief [Non-blocking] Whether the robot has come to a complete stop. @@ -128,7 +129,7 @@ class Robot * @note Possible log level tags are: [info], [warning], [error], and [critical]. * @warning Messages before the last successful instantiation of this class are not available. */ - std::vector mu_log() const; + const std::vector mu_log() const; //======================================= SYSTEM CONTROL ======================================= /** @@ -140,9 +141,21 @@ class Robot */ void Enable(); + /** + * @brief [Blocking] Force robot brakes to engage or release during normal operation. + * Restrictions apply, see warning. + * @param[in] engage True: engage brakes, false: release brakes. + * @throw std::logic_error if the connected robot is not a medical one or the robot is moving. + * @throw std::runtime_error if failed to engage/release the brakes. + * @note This function blocks until the brakes are successfully engaged/released. + * @warning This function is accessible only if a) the connected robot is a medical one AND + * b) the robot is not moving. + */ + void Brake(bool engage); + /** * @brief [Blocking] Switch to a new control mode and wait until mode transition is finished. - * @param[in] mode flexiv::Mode enum. + * @param[in] mode flexiv::rdk::Mode enum. * @throw std::invalid_argument if the requested mode is invalid or unlicensed. * @throw std::logic_error if robot is in an unknown control mode or is not operational. * @throw std::runtime_error if failed to transit the robot into the specified control mode @@ -155,20 +168,25 @@ class Robot void SwitchMode(Mode mode); /** - * @brief [Blocking] Stop the robot and transit robot mode to Idle. + * @brief [Blocking] Stop the robot and transit robot mode to IDLE. * @throw std::runtime_error if failed to stop the robot. * @note This function blocks until the robot comes to a complete stop. */ void Stop(); /** - * @brief [Blocking] Clear minor fault of the robot. - * @return True: successfully cleared fault, false: cannot clear fault. + * @brief [Blocking] Try to clear minor or critical fault of the robot without a power cycle. + * @param[in] timeout_sec Maximum time in seconds to wait for the fault to be successfully + * cleared. Normally, a minor fault should take no more than 3 seconds to clear, and a critical + * fault should take no more than 30 seconds to clear. + * @return True: successfully cleared fault, false: failed to clear fault. * @throw std::runtime_error if failed to deliver the request to the connected robot. - * @note This function blocks until robot fault is successfully cleared or maximum number of - * attempts is reached. + * @note This function blocks until the fault is successfully cleared or [timeout_sec] has + * elapsed. + * @warning Clearing a critical fault through this function without a power cycle requires a + * dedicated device, which may not be installed in older robot models. */ - bool ClearFault(); + bool ClearFault(unsigned int timeout_sec = 30); /** * @brief [Blocking] Run automatic recovery to bring joints that are outside the allowed @@ -176,7 +194,7 @@ class Robot * @throw std::runtime_error if failed to enter automatic recovery mode. * @note Refer to user manual for more details. * @note This function blocks until the automatic recovery process is finished. - * @see IsRecoveryState(). + * @see recovery(). */ void RunAutoRecovery(); @@ -205,7 +223,7 @@ class Robot * @throw std::runtime_error if failed to deliver the request to the connected robot. * @note Applicable control mode(s): NRT_PLAN_EXECUTION. * @note This function blocks until the request is successfully delivered. - * @note IsBusy() can be used to check if a plan task has finished. + * @note busy() can be used to check if a plan task has finished. */ void ExecutePlan(unsigned int index, bool continue_exec = false); @@ -218,7 +236,7 @@ class Robot * @throw std::runtime_error if failed to deliver the request to the connected robot. * @note Applicable control mode(s): NRT_PLAN_EXECUTION. * @note This function blocks until the request is successfully delivered. - * @note IsBusy() can be used to check if a plan task has finished. + * @note busy() can be used to check if a plan task has finished. */ void ExecutePlan(const std::string& name, bool continue_exec = false); @@ -238,7 +256,7 @@ class Robot * @throw std::runtime_error if failed to get a reply from the connected robot. * @note This function blocks until a reply is received. */ - std::vector plan_list() const; + const std::vector plan_list() const; /** * @brief [Blocking] Get detailed information about the currently executing plan. Contains @@ -249,12 +267,12 @@ class Robot * @note Applicable control mode(s): NRT_PLAN_EXECUTION. * @note This function blocks until a reply is received. */ - PlanInfo plan_info() const; + const PlanInfo plan_info() const; /** * @brief [Blocking] Set global variables for the robot by specifying name and value. * @param[in] global_vars Command to set global variables using the format: - * globalVar1=value(s), globalVar2=value(s), ... + * global_var1=value(s), global_var2=value(s), ... * @throw std::length_error if size of global_vars exceeds the limit (10 Kb). * @throw std::logic_error if robot is not in the correct control mode. * @throw std::runtime_error if failed to deliver the request to the connected robot. @@ -272,7 +290,7 @@ class Robot * @throw std::runtime_error if failed to get a reply from the connected robot. * @note This function blocks until a reply is received. */ - std::vector global_variables() const; + const std::vector global_variables() const; /** * @brief [Blocking] Enable or disable the breakpoint mode during plan execution. When enabled, @@ -315,7 +333,7 @@ class Robot * Primitives documentation for exact unit definition. * @warning Some primitives may not terminate automatically and require users to manually * terminate them based on specific primitive states, for example, most [Move] primitives. In - * such case, IsBusy() will stay true even if it seems everything is done for that primitive. + * such case, busy() will stay true even if it seems everything is done for that primitive. */ void ExecutePrimitive(const std::string& pt_cmd); @@ -326,7 +344,7 @@ class Robot * invalid. * @note This function blocks until a reply is received. */ - std::vector primitive_states() const; + const std::vector primitive_states() const; //==================================== DIRECT JOINT CONTROL ==================================== /** @@ -337,59 +355,94 @@ class Robot * @param[in] enable_soft_limits Enable/disable soft limits to keep the joints from moving * outside allowed position range, which will trigger a safety fault that requires recovery * operation. + * @throw std::invalid_argument if size of any input vector does not match robot DoF. * @throw std::logic_error if robot is not in the correct control mode. * @throw std::runtime_error if number of timeliness failures has reached limit. * @note Applicable control mode(s): RT_JOINT_TORQUE. * @note Real-time (RT). * @warning Always stream smooth and continuous commands to avoid sudden movements. */ - void StreamJointTorque(const std::array& torques, - bool enable_gravity_comp = true, bool enable_soft_limits = true); + void StreamJointTorque(const std::vector& torques, bool enable_gravity_comp = true, + bool enable_soft_limits = true); /** * @brief [Non-blocking] Continuously stream joint position, velocity, and acceleration command - * to the robot. + * to the robot. The commands are tracked by either the joint impedance controller or the joint + * position controller, depending on the control mode. * @param[in] positions Target joint positions: \f$ q_d \in \mathbb{R}^{n \times 1} \f$. Unit: * \f$ [rad] \f$. * @param[in] velocities Target joint velocities: \f$ \dot{q}_d \in \mathbb{R}^{n \times 1} * \f$. Unit: \f$ [rad/s] \f$. * @param[in] accelerations Target joint accelerations: \f$ \ddot{q}_d \in \mathbb{R}^{n \times * 1} \f$. Unit: \f$ [rad/s^2] \f$. + * @throw std::invalid_argument if size of any input vector does not match robot DoF. * @throw std::logic_error if robot is not in the correct control mode. * @throw std::runtime_error if number of timeliness failures has reached limit. - * @note Applicable control mode(s): RT_JOINT_POSITION. + * @note Applicable control mode(s): RT_JOINT_IMPEDANCE, RT_JOINT_POSITION. * @note Real-time (RT). * @warning Always stream smooth and continuous commands to avoid sudden movements. + * @see SetJointImpedance(). */ - void StreamJointPosition(const std::array& positions, - const std::array& velocities, - const std::array& accelerations); + void StreamJointPosition(const std::vector& positions, + const std::vector& velocities, const std::vector& accelerations); /** - * @brief [Non-blocking] Discretely send joint position, velocity, and acceleration command. The - * robot's internal motion generator will smoothen the discrete commands. - * @param[in] positions Target joint positions: \f$ q_d \in \mathbb{R}^{DOF \times 1} \f$. Unit: + * @brief [Non-blocking] Discretely send joint position, velocity, and acceleration command to + * the robot. The robot's internal motion generator will smoothen the discrete commands, which + * are tracked by either the joint impedance controller or the joint position controller, + * depending on the control mode. + * @param[in] positions Target joint positions: \f$ q_d \in \mathbb{R}^{n \times 1} \f$. Unit: * \f$ [rad] \f$. - * @param[in] velocities Target joint velocities: \f$ \dot{q}_d \in \mathbb{R}^{DOF \times 1} + * @param[in] velocities Target joint velocities: \f$ \dot{q}_d \in \mathbb{R}^{n \times 1} * \f$. Each joint will maintain this amount of velocity when it reaches the target position. * Unit: \f$ [rad/s] \f$. - * @param[in] accelerations Target joint accelerations: \f$ \ddot{q}_d \in \mathbb{R}^{DOF - * \times 1} \f$. Each joint will maintain this amount of acceleration when it reaches the - * target position. Unit: \f$ [rad/s^2] \f$. + * @param[in] accelerations Target joint accelerations: \f$ \ddot{q}_d \in \mathbb{R}^{n \times + * 1} \f$. Each joint will maintain this amount of acceleration when it reaches the target + * position. Unit: \f$ [rad/s^2] \f$. * @param[in] max_vel Maximum joint velocities for the planned trajectory: \f$ \dot{q}_{max} \in * \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad/s] \f$. * @param[in] max_acc Maximum joint accelerations for the planned trajectory: \f$ \ddot{q}_{max} * \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad/s^2] \f$. + * @throw std::invalid_argument if size of any input vector does not match robot DoF. * @throw std::logic_error if robot is not in the correct control mode. - * @note Applicable control mode(s): NRT_JOINT_POSITION. + * @note Applicable control mode(s): NRT_JOINT_IMPEDANCE, NRT_JOINT_POSITION. * @warning Calling this function a second time while the motion from the previous call is still * ongoing will trigger an online re-planning of the joint trajectory, such that the previous * command is aborted and the new command starts to execute. + * @see SetJointImpedance(). + */ + void SendJointPosition(const std::vector& positions, + const std::vector& velocities, const std::vector& accelerations, + const std::vector& max_vel, const std::vector& max_acc); + + /** + * @brief [Non-blocking] Set impedance properties of the robot's joint motion controller used in + * the joint impedance control modes. + * @param[in] K_q Joint motion stiffness: \f$ K_q \in \mathbb{R}^{n \times 1} \f$. + * Setting motion stiffness of a joint axis to 0 will make this axis free-floating. Valid range: + * [0, RobotInfo::K_q_nom]. Unit: \f$ [Nm/rad] \f$. + * @param[in] Z_q Joint motion damping ratio: \f$ Z_q \in \mathbb{R}^{n \times 1} \f$. + * Valid range: [0.3, 0.8]. The nominal (safe) value is provided as default. + * @throw std::invalid_argument if [K_q] or [Z_q] contains any value outside the valid range or + * size of any input vector does not match robot DoF. + * @throw std::logic_error if robot is not in the correct control mode. + * @note Applicable control mode(s): RT_JOINT_IMPEDANCE, NRT_JOINT_IMPEDANCE. + * @warning The robot will automatically reset to its nominal impedance properties upon + * re-entering the applicable control modes. + * @warning Changing damping ratio [Z_q] to a non-nominal value may lead to performance and + * stability issues, please use with caution. + * @see ResetJointImpedance(). */ - void SendJointPosition(const std::array& positions, - const std::array& velocities, - const std::array& accelerations, - const std::array& max_vel, const std::array& max_acc); + void SetJointImpedance(const std::vector& K_q, + const std::vector& Z_q = {0.7, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7}); + + /** + * @brief [Non-blocking] Reset impedance properties of the robot's joint motion controller to + * nominal values. + * @note Applicable control mode(s): RT_JOINT_IMPEDANCE, NRT_JOINT_IMPEDANCE. + * @see SetJointImpedance(). + */ + void ResetJointImpedance(); //================================== DIRECT CARTESIAN CONTROL ================================== /** @@ -425,12 +478,12 @@ class Robot * Use SetForceControlAxis() to enable force control for one or more Cartesian axes and leave * the rest axes motion-controlled, then provide target pose for the motion-controlled axes and * target wrench for the force-controlled axes. - * @see SetCartesianStiffness(), SetMaxContactWrench(), SetNullSpacePosture(), + * @see SetCartesianImpedance(), SetMaxContactWrench(), SetNullSpacePosture(), * SetForceControlAxis(), SetForceControlFrame(), SetPassiveForceControl(). */ void StreamCartesianMotionForce(const std::array& pose, - const std::array& wrench = {}, - const std::array& acceleration = {}); + const std::array& wrench = {}, + const std::array& acceleration = {}); /** * @brief [Non-blocking] Discretely send Cartesian motion and/or force command for the robot to @@ -466,34 +519,44 @@ class Robot * Use SetForceControlAxis() to enable force control for one or more Cartesian axes and leave * the rest axes motion-controlled, then provide target pose for the motion-controlled axes and * target wrench for the force-controlled axes. - * @see SetCartesianStiffness(), SetMaxContactWrench(), SetNullSpacePosture(), + * @see SetCartesianImpedance(), SetMaxContactWrench(), SetNullSpacePosture(), * SetForceControlAxis(), SetForceControlFrame(), SetPassiveForceControl(). */ void SendCartesianMotionForce(const std::array& pose, - const std::array& wrench = {}, double max_linear_vel = 0.5, + const std::array& wrench = {}, double max_linear_vel = 0.5, double max_angular_vel = 1.0, double max_linear_acc = 2.0, double max_angular_acc = 5.0); /** - * @brief [Non-blocking] Set motion stiffness for the Cartesian motion-force control modes. - * @param[in] stiffness Cartesian motion stiffness: \f$ K_d \in \mathbb{R}^{6 \times 1} \f$. + * @brief [Non-blocking] Set impedance properties of the robot's Cartesian motion controller + * used in the Cartesian motion-force control modes. + * @param[in] K_x Cartesian motion stiffness: \f$ K_x \in \mathbb{R}^{6 \times 1} \f$. * Setting motion stiffness of a motion-controlled Cartesian axis to 0 will make this axis * free-floating. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ linear stiffness and \f$ * \mathbb{R}^{3 \times 1} \f$ angular stiffness: \f$ [k_x, k_y, k_z, k_{Rx}, k_{Ry}, k_{Rz}]^T - * \f$. Valid range: [0, RobotInfo::nominal_K]. Unit: \f$ [N/m]~[Nm/rad] \f$. - * @throw std::invalid_argument if [stiffness] contains any value outside the valid range. + * \f$. Valid range: [0, RobotInfo::K_x_nom]. Unit: \f$ [N/m]~[Nm/rad] \f$. + * @param[in] Z_x Cartesian motion damping ratio: \f$ Z_x \in \mathbb{R}^{6 \times 1} \f$. + * Consists of \f$ \mathbb{R}^{3 \times 1} \f$ linear damping ratio and \f$ \mathbb{R}^{3 \times + * 1} \f$ angular damping ratio: \f$ [\zeta_x, \zeta_y, \zeta_z, \zeta_{Rx}, \zeta_{Ry}, + * \zeta_{Rz}]^T \f$. Valid range: [0.3, 0.8]. The nominal (safe) value is provided as default. + * @throw std::invalid_argument if [K_x] or [Z_x] contains any value outside the valid range. * @throw std::logic_error if robot is not in the correct control mode. * @note Applicable control mode(s): RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE. - * @warning The robot will automatically reset to its nominal stiffness upon re-entering the - * applicable control modes. + * @warning The robot will automatically reset to its nominal impedance properties upon + * re-entering the applicable control modes. + * @warning Changing damping ratio [Z_x] to a non-nominal value may lead to performance and + * stability issues, please use with caution. + * @see ResetCartesianImpedance(). */ - void SetCartesianStiffness(const std::array& stiffness); + void SetCartesianImpedance(const std::array& K_x, + const std::array& Z_x = {0.7, 0.7, 0.7, 0.7, 0.7, 0.7}); /** - * @brief [Non-blocking] Reset motion stiffness for the Cartesian motion-force control modes to - * nominal value. + * @brief [Non-blocking] Reset impedance properties of the robot's Cartesian motion controller + * to nominal values. * @note Applicable control mode(s): RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE. + * @see SetCartesianImpedance(). */ - void ResetCartesianStiffness(); + void ResetCartesianImpedance(); /** * @brief [Non-blocking] Set maximum contact wrench for the motion control part of the Cartesian @@ -511,8 +574,9 @@ class Robot * re-entering the applicable control modes. * @warning The maximum contact wrench regulation cannot be enabled if any of the rotational * Cartesian axes is enabled for moment control. + * @see ResetMaxContactWrench(). */ - void SetMaxContactWrench(const std::array& max_wrench); + void SetMaxContactWrench(const std::array& max_wrench); /** * @brief [Non-blocking] Reset max contact wrench regulation to nominal state, i.e. disabled. @@ -527,20 +591,21 @@ class Robot * \f$ q_{ns} \in \mathbb{R}^{n \times 1} \f$. Valid range: [RobotInfo::q_min, * RobotInfo::q_max]. Unit: \f$ [rad] \f$. * @throw std::invalid_argument if [preferred_positions] contains any value outside the valid - * range. + * range or size of any input vector does not match robot DoF. * @throw std::logic_error if robot is not in the correct control mode. * @note Applicable control mode(s): RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE. * @warning Upon entering the applicable control modes, the robot will automatically set its * current joint positions as the preferred joint positions. * @par Null-space posture control - * Similar to human arm, a robotic arm with redundant joint-space degree(s) of freedom (DOF > 6) + * Similar to human arm, a robotic arm with redundant joint-space degree(s) of freedom (DoF > 6) * can change its overall posture without affecting the ongoing primary task. This is achieved * through a technique called "null-space control". After the preferred joint positions for a * desired robot posture is set using this function, the robot's null-space control module will * try to pull the arm as close to this posture as possible without affecting the primary * Cartesian motion-force control task. + * @see ResetNullSpacePosture(). */ - void SetNullSpacePosture(const std::array& preferred_positions); + void SetNullSpacePosture(const std::vector& preferred_positions); /** * @brief [Non-blocking] Reset preferred joint positions to the ones automatically recorded when @@ -571,8 +636,8 @@ class Robot * @warning Upon disconnection, force control axes will be reset to all disabled and maximum * linear velocity in force control axes will be reset to 1.0 m/s. */ - void SetForceControlAxis(const std::array& enabled_axis, - const std::array& max_linear_vel = {1.0, 1.0, 1.0}); + void SetForceControlAxis(const std::array& enabled_axis, + const std::array& max_linear_vel = {1.0, 1.0, 1.0}); /** * @brief [Blocking] Set force control reference frame for the Cartesian motion-force control @@ -646,7 +711,7 @@ class Robot * @return Digital input readings array whose index corresponds to the digital input port index. * True: port high, false: port low. */ - std::array ReadDigitalInput(); + const std::array ReadDigitalInput(); private: class Impl; @@ -659,6 +724,7 @@ class Robot friend class FileIO; }; +} /* namespace rdk */ } /* namespace flexiv */ -#endif /* FLEXIVRDK_ROBOT_H_ */ +#endif /* FLEXIV_RDK_ROBOT_HPP_ */ diff --git a/include/flexiv/scheduler.h b/include/flexiv/rdk/scheduler.hpp similarity index 94% rename from include/flexiv/scheduler.h rename to include/flexiv/rdk/scheduler.hpp index 90492f06..5a3dea25 100644 --- a/include/flexiv/scheduler.h +++ b/include/flexiv/rdk/scheduler.hpp @@ -1,16 +1,17 @@ /** - * @file scheduler.h - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * @file scheduler.hpp + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. */ -#ifndef FLEXIVRDK_SCHEDULER_H_ -#define FLEXIVRDK_SCHEDULER_H_ +#ifndef FLEXIV_RDK_SCHEDULER_HPP_ +#define FLEXIV_RDK_SCHEDULER_HPP_ #include #include #include namespace flexiv { +namespace rdk { /** * @class Scheduler @@ -55,7 +56,7 @@ class Scheduler * @throw std::runtime_error if an error is triggered by the client computer. * @note Setting CPU affinity on macOS has no effect, as its Mach kernel takes full control of * thread placement so CPU binding is not supported. - * @warning Calling this method after start() is not allowed. + * @warning Calling this function after start() is not allowed. * @warning For maximum scheduling performance, setting CPU affinity to 0 or 1 is not allowed: * core 0 is usually the default core for system processes and can be crowded; core 1 is * reserved for the scheduler itself. @@ -108,6 +109,7 @@ class Scheduler std::unique_ptr pimpl_; }; +} /* namespace rdk */ } /* namespace flexiv */ -#endif /* FLEXIVRDK_SCHEDULER_H_ */ +#endif /* FLEXIV_RDK_SCHEDULER_HPP_ */ diff --git a/include/flexiv/tool.h b/include/flexiv/rdk/tool.hpp similarity index 90% rename from include/flexiv/tool.h rename to include/flexiv/rdk/tool.hpp index df23df38..6b928edd 100644 --- a/include/flexiv/tool.h +++ b/include/flexiv/rdk/tool.hpp @@ -1,14 +1,15 @@ /** - * @file tool.h - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * @file tool.hpp + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. */ -#ifndef FLEXIVRDK_TOOL_H_ -#define FLEXIVRDK_TOOL_H_ +#ifndef FLEXIV_RDK_TOOL_HPP_ +#define FLEXIV_RDK_TOOL_HPP_ -#include "robot.h" +#include "robot.hpp" namespace flexiv { +namespace rdk { /** * @class Tool @@ -21,7 +22,7 @@ class Tool public: /** * @brief [Non-blocking] Create an instance and initialize tool update interface. - * @param[in] robot Reference to the instance of flexiv::Robot. + * @param[in] robot Reference to the instance of flexiv::rdk::Robot. * @throw std::runtime_error if the initialization sequence failed. */ Tool(const Robot& robot); @@ -33,16 +34,16 @@ class Tool * @throw std::runtime_error if failed to get a reply from the connected robot. * @note This function blocks until a reply is received. */ - std::vector list() const; + const std::vector list() const; /** * @brief [Blocking] Get name of the tool that the robot is currently using. * @return Name of the current tool. Return "Flange" if there's no active tool. - * @note Applicable control modes: All. * @throw std::runtime_error if failed to get a reply from the connected robot. + * @note Applicable control modes: All. * @note This function blocks until a reply is received. */ - std::string name() const; + const std::string name() const; /** * @brief [Blocking] Whether the specified tool exists in the tools pool. @@ -56,21 +57,21 @@ class Tool /** * @brief [Blocking] Get parameters of the tool that the robot is currently using. * @return Parameters result. - * @note Applicable control modes: All. * @throw std::runtime_error if failed to get a reply from the connected robot. + * @note Applicable control modes: All. * @note This function blocks until a reply is received. */ - ToolParams params() const; + const ToolParams params() const; /** * @brief [Blocking] Get parameters of an existing tool. * @param[in] name Name of the tool to get parameters for, must exist in the tools pool. * @return Parameters result. - * @note Applicable control modes: All. * @throw std::runtime_error if failed to get a reply from the connected robot. + * @note Applicable control modes: All. * @note This function blocks until a reply is received. */ - ToolParams params(const std::string& name) const; + const ToolParams params(const std::string& name) const; /** * @brief [Blocking] Add a new tool with user-specified parameters to the tools pool. @@ -120,6 +121,7 @@ class Tool std::unique_ptr pimpl_; }; +} /* namespace rdk */ } /* namespace flexiv */ -#endif /* FLEXIVRDK_TOOL_H_ */ +#endif /* FLEXIV_RDK_TOOL_HPP_ */ diff --git a/include/flexiv/utility.h b/include/flexiv/rdk/utility.hpp similarity index 75% rename from include/flexiv/utility.h rename to include/flexiv/rdk/utility.hpp index 4491ae87..53ba57e2 100644 --- a/include/flexiv/utility.h +++ b/include/flexiv/rdk/utility.hpp @@ -1,15 +1,16 @@ /** - * @file utility.h - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * @file utility.hpp + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. */ -#ifndef FLEXIVRDK_UTILITY_H_ -#define FLEXIVRDK_UTILITY_H_ +#ifndef FLEXIV_RDK_UTILITY_HPP_ +#define FLEXIV_RDK_UTILITY_HPP_ #include #include namespace flexiv { +namespace rdk { namespace utility { /** @@ -54,25 +55,26 @@ inline std::array Rad2Deg(const std::array& rad_arr) } /** - * @brief Convert a std::array to a string. - * @param[in] arr std::array of any type and size. - * @param[in] decimal Decimal places to keep for each number in the array. + * @brief Convert an std::vector to a string. + * @param[in] vec std::vector of any type and size. + * @param[in] decimal Decimal places to keep for each number in the vector. * @param[in] trailing_space Whether to include a space after the last element. - * @return A string with format "arr[0] arr[1] ... arr[n] ", i.e. each element followed by a space, + * @param[in] separator Character to separate between numbers. + * @return A string with format "vec[0] vec[1] ... vec[n] ", i.e. each element followed by a space, * including the last one if trailing_space = true. */ -template -inline std::string Arr2Str( - const std::array& arr, size_t decimal = 3, bool trailing_space = true) +template +inline std::string Vec2Str(const std::vector& vec, size_t decimal = 3, + bool trailing_space = true, const std::string& separator = " ") { - auto padding = ""; + std::string padding = ""; std::stringstream ss; ss.precision(decimal); ss << std::fixed; - for (const auto& v : arr) { + for (const auto& v : vec) { ss << padding << v; - padding = " "; + padding = separator; } if (trailing_space) { @@ -81,6 +83,24 @@ inline std::string Arr2Str( return ss.str(); } +/** + * @brief Convert an std::array to a string. + * @param[in] arr std::array of any type and size. + * @param[in] decimal Decimal places to keep for each number in the array. + * @param[in] trailing_space Whether to include a space after the last element. + * @param[in] separator Character to separate between numbers. + * @return A string with format "arr[0] arr[1] ... arr[n] ", i.e. each element followed by a space, + * including the last one if trailing_space = true. + */ +template +inline std::string Arr2Str(const std::array& arr, size_t decimal = 3, + bool trailing_space = true, const std::string& separator = " ") +{ + std::vector vec(N); + std::copy(arr.begin(), arr.end(), vec.begin()); + return Vec2Str(vec, decimal, trailing_space, separator); +} + /** * @brief Check if any provided strings exist in the program arguments. * @param[in] argc Argument count passed to main() of the program. @@ -144,6 +164,7 @@ inline std::string ParsePtStates( } } /* namespace utility */ +} /* namespace rdk */ } /* namespace flexiv */ -#endif /* FLEXIVRDK_UTILITY_H_ */ +#endif /* FLEXIV_RDK_UTILITY_HPP_ */ diff --git a/lib/flexiv_rdk.win_amd64.lib b/lib/flexiv_rdk.win_amd64.lib index eadf17c9..b047f555 100644 Binary files a/lib/flexiv_rdk.win_amd64.lib and b/lib/flexiv_rdk.win_amd64.lib differ diff --git a/lib/libflexiv_rdk.aarch64-linux-gnu.a b/lib/libflexiv_rdk.aarch64-linux-gnu.a index c3b12da0..26d0448b 100644 Binary files a/lib/libflexiv_rdk.aarch64-linux-gnu.a and b/lib/libflexiv_rdk.aarch64-linux-gnu.a differ diff --git a/lib/libflexiv_rdk.arm64-darwin.a b/lib/libflexiv_rdk.arm64-darwin.a index 27e2ac33..de531b09 100644 Binary files a/lib/libflexiv_rdk.arm64-darwin.a and b/lib/libflexiv_rdk.arm64-darwin.a differ diff --git a/lib/libflexiv_rdk.x86_64-linux-gnu.a b/lib/libflexiv_rdk.x86_64-linux-gnu.a index 9623807c..8bbc0fb8 100644 Binary files a/lib/libflexiv_rdk.x86_64-linux-gnu.a and b/lib/libflexiv_rdk.x86_64-linux-gnu.a differ diff --git a/lib_py/flexivrdk.cp310-win_amd64.pyd b/lib_py/flexivrdk.cp310-win_amd64.pyd index f579ec22..7c758a39 100644 Binary files a/lib_py/flexivrdk.cp310-win_amd64.pyd and b/lib_py/flexivrdk.cp310-win_amd64.pyd differ diff --git a/lib_py/flexivrdk.cp312-win_amd64.pyd b/lib_py/flexivrdk.cp312-win_amd64.pyd index 26d4b66f..e21703e5 100644 Binary files a/lib_py/flexivrdk.cp312-win_amd64.pyd and b/lib_py/flexivrdk.cp312-win_amd64.pyd differ diff --git a/lib_py/flexivrdk.cpython-310-aarch64-linux-gnu.so b/lib_py/flexivrdk.cpython-310-aarch64-linux-gnu.so index e0e81924..1b02c50b 100755 Binary files a/lib_py/flexivrdk.cpython-310-aarch64-linux-gnu.so and b/lib_py/flexivrdk.cpython-310-aarch64-linux-gnu.so differ diff --git a/lib_py/flexivrdk.cpython-310-darwin.so b/lib_py/flexivrdk.cpython-310-darwin.so index a6a0301d..93e3a94d 100755 Binary files a/lib_py/flexivrdk.cpython-310-darwin.so and b/lib_py/flexivrdk.cpython-310-darwin.so differ diff --git a/lib_py/flexivrdk.cpython-310-x86_64-linux-gnu.so b/lib_py/flexivrdk.cpython-310-x86_64-linux-gnu.so index 87c1a1c5..f88c96b6 100755 Binary files a/lib_py/flexivrdk.cpython-310-x86_64-linux-gnu.so and b/lib_py/flexivrdk.cpython-310-x86_64-linux-gnu.so differ diff --git a/lib_py/flexivrdk.cpython-312-aarch64-linux-gnu.so b/lib_py/flexivrdk.cpython-312-aarch64-linux-gnu.so index c96481f7..cc799a6b 100755 Binary files a/lib_py/flexivrdk.cpython-312-aarch64-linux-gnu.so and b/lib_py/flexivrdk.cpython-312-aarch64-linux-gnu.so differ diff --git a/lib_py/flexivrdk.cpython-312-darwin.so b/lib_py/flexivrdk.cpython-312-darwin.so index 7172c79e..b7ff59c8 100755 Binary files a/lib_py/flexivrdk.cpython-312-darwin.so and b/lib_py/flexivrdk.cpython-312-darwin.so differ diff --git a/lib_py/flexivrdk.cpython-312-x86_64-linux-gnu.so b/lib_py/flexivrdk.cpython-312-x86_64-linux-gnu.so index fedb9973..a4a5e762 100755 Binary files a/lib_py/flexivrdk.cpython-312-x86_64-linux-gnu.so and b/lib_py/flexivrdk.cpython-312-x86_64-linux-gnu.so differ diff --git a/lib_py/flexivrdk.cpython-38-x86_64-linux-gnu.so b/lib_py/flexivrdk.cpython-38-x86_64-linux-gnu.so index c0af57c9..a92ff090 100755 Binary files a/lib_py/flexivrdk.cpython-38-x86_64-linux-gnu.so and b/lib_py/flexivrdk.cpython-38-x86_64-linux-gnu.so differ diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index a08526d9..5df1acc0 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -15,7 +15,6 @@ endif() # Tests for Windows set(TEST_LIST test_dynamics_engine - test_log ) # Additional tests for Linux and Mac diff --git a/test/test_dynamics_engine.cpp b/test/test_dynamics_engine.cpp index bf52a88a..9cd57908 100644 --- a/test/test_dynamics_engine.cpp +++ b/test/test_dynamics_engine.cpp @@ -1,16 +1,15 @@ /** * @test test_dynamics_engine.cpp * A test to evaluate the dynamics engine (J, M, G), with and without end-effector tool. - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ -#include -#include -#include -#include -#include - +#include +#include +#include +#include +#include #include #include @@ -32,7 +31,7 @@ struct GroundTruth } /** Step the dynamics engine once */ -void StepDynamics(flexiv::Robot& robot, flexiv::Model& model, const GroundTruth& ref) +void StepDynamics(flexiv::rdk::Robot& robot, flexiv::rdk::Model& model, const GroundTruth& ref) { // Mark timer start point auto tic = std::chrono::high_resolution_clock::now(); @@ -87,12 +86,9 @@ void PrintHelp() int main(int argc, char* argv[]) { - // Log object for printing message with timestamp and coloring - flexiv::Log log; - // Parse Parameters //============================================================================================== - if (argc < 2 || flexiv::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { + if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { PrintHelp(); return 1; } @@ -104,31 +100,31 @@ int main(int argc, char* argv[]) // RDK Initialization //========================================================================================== // Instantiate robot interface - flexiv::Robot robot(robot_sn); + flexiv::rdk::Robot robot(robot_sn); // Clear fault on the connected robot if any if (robot.fault()) { - log.Warn("Fault occurred on the connected robot, trying to clear ..."); + spdlog::warn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault if (!robot.ClearFault()) { - log.Error("Fault cannot be cleared, exiting ..."); + spdlog::error("Fault cannot be cleared, exiting ..."); return 1; } - log.Info("Fault on the connected robot is cleared"); + spdlog::info("Fault on the connected robot is cleared"); } // Enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ..."); + spdlog::info("Enabling robot ..."); robot.Enable(); // Wait for the robot to become operational while (!robot.operational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); } - log.Info("Robot is now operational"); + spdlog::info("Robot is now operational"); // Set mode after robot is operational - robot.SwitchMode(flexiv::Mode::NRT_PLAN_EXECUTION); + robot.SwitchMode(flexiv::rdk::Mode::NRT_PLAN_EXECUTION); // Bring Robot To Home //========================================================================================== @@ -140,14 +136,14 @@ int main(int argc, char* argv[]) } while (robot.busy()); // Put mode back to IDLE - robot.SwitchMode(flexiv::Mode::IDLE); + robot.SwitchMode(flexiv::rdk::Mode::IDLE); // Test Dynamics Engine without Tool //========================================================================================== - log.Info(">>>>> Test 1: no end-effector tool <<<<<"); + spdlog::info(">>>>> Test 1: no end-effector tool <<<<<"); // Instantiate dynamics engine - flexiv::Model model(robot); + flexiv::rdk::Model model(robot); // Ground truth from MATLAB without robot tool GroundTruth ref; @@ -180,7 +176,7 @@ int main(int argc, char* argv[]) // Test Dynamics Engine with Tool //========================================================================================== - log.Info(">>>>> Test 2: with end-effector tool <<<<<"); + spdlog::info(">>>>> Test 2: with end-effector tool <<<<<"); // Ground truth from MATLAB with robot tool // clang-format off @@ -199,11 +195,11 @@ int main(int argc, char* argv[]) // clang-format on // Instantiate tool interface - flexiv::Tool tool(robot); + flexiv::rdk::Tool tool(robot); // Set name and parameters for the test tool std::string tool_name = "DynamicsTestTool"; - flexiv::ToolParams tool_params; + flexiv::rdk::ToolParams tool_params; tool_params.mass = 0.9; tool_params.CoM = {0.0, 0.0, 0.057}; tool_params.inertia = {2.768e-03, 3.149e-03, 5.64e-04, 0.0, 0.0, 0.0}; @@ -211,19 +207,22 @@ int main(int argc, char* argv[]) // Remove any existing tool with the same name if (tool.exist(tool_name)) { + spdlog::warn("Tool with the same name [{}] already exists, removing it now", tool_name); + // Switch to other tool or no tool (Flange) before removing the current tool + tool.Switch("Flange"); tool.Remove(tool_name); } // Add the test tool - log.Info("Adding test tool [" + tool_name + "] to the robot"); + spdlog::info("Adding test tool [{}] to the robot", tool_name); tool.Add(tool_name, tool_params); // Switch to the newly added test tool, i.e. set it as the active tool - log.Info("Switching to test tool [" + tool_name + "]"); + spdlog::info("Switching to test tool [{}]", tool_name); tool.Switch(tool_name); // Get and print the current active tool, should be the test tool - log.Info("Current active tool: " + tool.name()); + spdlog::info("Current active tool: {}", tool.name()); // Reload robot + tool model using the latest data synced from the connected robot model.Reload(); @@ -235,13 +234,16 @@ int main(int argc, char* argv[]) std::this_thread::sleep_for(std::chrono::milliseconds(100)); } + // Switch to other tool or no tool (Flange) before removing the current tool + tool.Switch("Flange"); + // Clean up by removing the test tool - log.Info("Removing tool [" + tool_name + "]"); + spdlog::info("Removing tool [{}]", tool_name); tool.Remove(tool_name); - log.Info("Program finished"); + spdlog::info("Program finished"); } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); return 1; } diff --git a/test/test_endurance.cpp b/test/test_endurance.cpp index 33017597..7a7b0281 100644 --- a/test/test_endurance.cpp +++ b/test/test_endurance.cpp @@ -2,14 +2,14 @@ * @test test_endurance.cpp * Endurance test running Cartesian impedance control to slowly sine-sweep near home for a duration * of user-specified hours. Raw data will be logged to CSV files continuously. - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -29,7 +29,7 @@ const double kSwingAmp = 0.1; const double kSwingFreq = 0.025; // = 10mm/s linear velocity /** Current Cartesian-space pose (position + rotation) of robot TCP */ -std::array g_curr_tcp_pose; +std::array g_curr_tcp_pose; /** Test duration converted from user-specified hours to loop counts */ uint64_t g_test_duration_loop_counts = 0; @@ -40,8 +40,8 @@ const unsigned int kLogDurationLoopCounts = 10 * 60 * 1000; // = 10 min/file /** Data to be logged in low-priority thread */ struct LogData { - std::array tcp_pose; - std::array tcp_force; + std::array tcp_pose; + std::array tcp_force; } g_log_data; /** Atomic signal to stop the test */ @@ -49,7 +49,7 @@ std::atomic g_stop = {false}; } void highPriorityTask( - flexiv::Robot& robot, flexiv::Log& log, const std::array& init_pose) + flexiv::rdk::Robot& robot, const std::array& init_pose) { // Local periodic loop counter static uint64_t loop_counter = 0; @@ -77,7 +77,7 @@ void highPriorityTask( } } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); g_stop = true; } } @@ -96,9 +96,6 @@ void lowPriorityTask() // CSV file counter (data during the test is divided to multiple files) unsigned int file_counter = 0; - // Log object for printing message with timestamp and coloring - flexiv::Log log; - // Wait for a while for the robot states data to be available std::this_thread::sleep_for(std::chrono::seconds(2)); @@ -112,7 +109,7 @@ void lowPriorityTask() if (loop_counter % kLogDurationLoopCounts == 0) { if (csv_file.is_open()) { csv_file.close(); - log.Info("Saved log file: " + csv_file_name); + spdlog::info("Saved log file: {}", csv_file_name); } // Increment log file counter @@ -124,9 +121,9 @@ void lowPriorityTask() // Open new log file csv_file.open(csv_file_name); if (csv_file.is_open()) { - log.Info("Created new log file: " + csv_file_name); + spdlog::info("Created new log file: {}", csv_file_name); } else { - log.Error("Failed to create log file: " + csv_file_name); + spdlog::error("Failed to create log file: {}", csv_file_name); } } @@ -146,11 +143,11 @@ void lowPriorityTask() // Check if the test duration has elapsed if (g_stop) { - log.Info("Test duration has elapsed, saving any open log file ..."); + spdlog::info("Test duration has elapsed, saving any open log file ..."); // Close log file if (csv_file.is_open()) { csv_file.close(); - log.Info("Saved log file: " + csv_file_name); + spdlog::info("Saved log file: {}", csv_file_name); } // Exit thread return; @@ -172,12 +169,9 @@ void PrintHelp() int main(int argc, char* argv[]) { - // log object for printing message with timestamp and coloring - flexiv::Log log; - // Parse Parameters //============================================================================================== - if (argc < 3 || flexiv::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { + if (argc < 3 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { PrintHelp(); return 1; } @@ -189,39 +183,38 @@ int main(int argc, char* argv[]) double test_hours = std::stof(argv[2]); // convert duration in hours to loop counts g_test_duration_loop_counts = (uint64_t)(test_hours * 3600.0 * 1000.0); - log.Info("Test duration: " + std::to_string(test_hours) - + " hours = " + std::to_string(g_test_duration_loop_counts) + " cycles"); + spdlog::info("Test duration: {} hours = {} cycles", test_hours, g_test_duration_loop_counts); try { // RDK Initialization //========================================================================================== // Instantiate robot interface - flexiv::Robot robot(robot_sn); + flexiv::rdk::Robot robot(robot_sn); // Clear fault on the connected robot if any if (robot.fault()) { - log.Warn("Fault occurred on the connected robot, trying to clear ..."); + spdlog::warn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault if (!robot.ClearFault()) { - log.Error("Fault cannot be cleared, exiting ..."); + spdlog::error("Fault cannot be cleared, exiting ..."); return 1; } - log.Info("Fault on the connected robot is cleared"); + spdlog::info("Fault on the connected robot is cleared"); } // enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ..."); + spdlog::info("Enabling robot ..."); robot.Enable(); // Wait for the robot to become operational while (!robot.operational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); } - log.Info("Robot is now operational"); + spdlog::info("Robot is now operational"); // Bring Robot To Home //========================================================================================== - robot.SwitchMode(flexiv::Mode::NRT_PLAN_EXECUTION); + robot.SwitchMode(flexiv::rdk::Mode::NRT_PLAN_EXECUTION); robot.ExecutePlan("PLAN-Home"); // Wait fot the plan to finish @@ -230,20 +223,19 @@ int main(int argc, char* argv[]) } while (robot.busy()); // Switch mode after robot is at home - robot.SwitchMode(flexiv::Mode::RT_CARTESIAN_MOTION_FORCE); + robot.SwitchMode(flexiv::rdk::Mode::RT_CARTESIAN_MOTION_FORCE); // Set initial pose to current TCP pose auto init_pose = robot.states().tcp_pose; - log.Info("Initial TCP pose set to [position 3x1, rotation (quaternion) 4x1]: " - + flexiv::utility::Arr2Str(init_pose)); + spdlog::info("Initial TCP pose set to [position 3x1, rotation (quaternion) 4x1]: " + + flexiv::rdk::utility::Arr2Str(init_pose)); g_curr_tcp_pose = init_pose; // Periodic Tasks //========================================================================================== - flexiv::Scheduler scheduler; + flexiv::rdk::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority - scheduler.AddTask( - std::bind(highPriorityTask, std::ref(robot), std::ref(log), std::ref(init_pose)), + scheduler.AddTask(std::bind(highPriorityTask, std::ref(robot), std::ref(init_pose)), "HP periodic", 1, scheduler.max_priority()); // Start all added tasks scheduler.Start(); @@ -255,7 +247,7 @@ int main(int argc, char* argv[]) low_priority_thread.join(); } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); return 1; } diff --git a/test/test_log.cpp b/test/test_log.cpp deleted file mode 100644 index 9b236ba9..00000000 --- a/test/test_log.cpp +++ /dev/null @@ -1,45 +0,0 @@ -/** - * @test test_log.cpp - * Test log functions of the flexiv::Log class - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. - * @author Flexiv - */ - -#include -#include - -#include -#include - -void PrintHelp() -{ - // clang-format off - std::cout << "Required arguments: None" << std::endl; - std::cout << "Optional arguments: None" << std::endl; - std::cout << std::endl; - // clang-format on -} - -int main(int argc, char* argv[]) -{ - if (flexiv::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { - PrintHelp(); - return 1; - } - - // log object for printing message with timestamp and coloring - flexiv::Log log; - - // print info message - log.Info("This is an INFO message with timestamp and GREEN coloring"); - std::this_thread::sleep_for(std::chrono::seconds(1)); - - // print warning message - log.Warn("This is a WARNING message with timestamp and YELLOW coloring"); - std::this_thread::sleep_for(std::chrono::seconds(1)); - - // print error message - log.Error("This is an ERROR message with timestamp and RED coloring"); - - return 0; -} diff --git a/test/test_loop_latency.cpp b/test/test_loop_latency.cpp index a4ef302b..647f1a6d 100644 --- a/test/test_loop_latency.cpp +++ b/test/test_loop_latency.cpp @@ -3,14 +3,14 @@ * A test to benchmark RDK's loop latency, including communication, computation, etc. The * workstation PC's serial port is used as reference, and the robot server's digital out port is * used as test target. - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -37,7 +37,7 @@ std::atomic g_stop_sched = {false}; } // callback function for realtime periodic task -void PeriodicTask(flexiv::Robot& robot, flexiv::Log& log) +void PeriodicTask(flexiv::rdk::Robot& robot) { // Loop counter static unsigned int loop_counter = 0; @@ -52,7 +52,7 @@ void PeriodicTask(flexiv::Robot& robot, flexiv::Log& log) // send signal at 1Hz switch (loop_counter % 1000) { case 0: { - log.Info( + spdlog::info( "Sending benchmark signal to both workstation PC's serial " "port and robot server's digital out port[0]"); break; @@ -64,7 +64,7 @@ void PeriodicTask(flexiv::Robot& robot, flexiv::Log& log) // signal workstation PC's serial port auto n = write(g_fd, "0", 1); if (n < 0) { - log.Error("Failed to write to serial port"); + spdlog::error("Failed to write to serial port"); } break; @@ -80,7 +80,7 @@ void PeriodicTask(flexiv::Robot& robot, flexiv::Log& log) loop_counter++; } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); g_stop_sched = true; } } @@ -100,12 +100,9 @@ void PrintHelp() int main(int argc, char* argv[]) { - // log object for printing message with timestamp and coloring - flexiv::Log log; - // Parse Parameters //============================================================================= - if (argc < 3 || flexiv::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { + if (argc < 3 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { PrintHelp(); return 1; } @@ -120,28 +117,28 @@ int main(int argc, char* argv[]) // RDK Initialization //============================================================================= // Instantiate robot interface - flexiv::Robot robot(robot_sn); + flexiv::rdk::Robot robot(robot_sn); // Clear fault on the connected robot if any if (robot.fault()) { - log.Warn("Fault occurred on the connected robot, trying to clear ..."); + spdlog::warn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault if (!robot.ClearFault()) { - log.Error("Fault cannot be cleared, exiting ..."); + spdlog::error("Fault cannot be cleared, exiting ..."); return 1; } - log.Info("Fault on the connected robot is cleared"); + spdlog::info("Fault on the connected robot is cleared"); } // enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ..."); + spdlog::info("Enabling robot ..."); robot.Enable(); // Wait for the robot to become operational while (!robot.operational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); } - log.Info("Robot is now operational"); + spdlog::info("Robot is now operational"); // Benchmark Signal //============================================================================= @@ -149,18 +146,18 @@ int main(int argc, char* argv[]) g_fd = open(serial_port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY | O_EXCL | O_CLOEXEC); if (g_fd == -1) { - log.Error("Unable to open serial port " + serial_port); + spdlog::error("Unable to open serial port [{}]", serial_port); } // print messages - log.Warn("Benchmark signal will be sent every 1 second"); + spdlog::warn("Benchmark signal will be sent every 1 second"); // Periodic Tasks //============================================================================= - flexiv::Scheduler scheduler; + flexiv::rdk::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority - scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::ref(log)), "HP periodic", 1, - scheduler.max_priority()); + scheduler.AddTask( + std::bind(PeriodicTask, std::ref(robot)), "HP periodic", 1, scheduler.max_priority()); // Start all added tasks scheduler.Start(); @@ -172,7 +169,7 @@ int main(int argc, char* argv[]) scheduler.Stop(); } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); return 1; } diff --git a/test/test_scheduler.cpp b/test/test_scheduler.cpp index f33b4564..4c003ac3 100644 --- a/test/test_scheduler.cpp +++ b/test/test_scheduler.cpp @@ -1,13 +1,13 @@ /** * @test test_scheduler.cpp * A test to evaluate RDK's internal real-time scheduler. - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ -#include -#include -#include +#include +#include +#include #include #include @@ -30,7 +30,7 @@ std::atomic g_stop_sched = {false}; } /** User-defined high-priority periodic task @ 1kHz */ -void highPriorityTask(flexiv::Log& log) +void highPriorityTask() { static unsigned int loop_counter = 0; @@ -61,13 +61,13 @@ void highPriorityTask(flexiv::Log& log) tic = std::chrono::high_resolution_clock::now(); } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); g_stop_sched = true; } } /** User-defined low-priority periodic task @1Hz */ -void lowPriorityTask(flexiv::Log& log) +void lowPriorityTask() { static uint64_t accumulated_time = 0; static uint64_t num_measures = 0; @@ -86,8 +86,8 @@ void lowPriorityTask(flexiv::Log& log) avg_interval = (float)accumulated_time / (float)num_measures; // print time interval of high-priority periodic task - log.Info("High-priority task interval (curr | avg) = " + std::to_string(measured_interval) - + " | " + std::to_string(avg_interval) + " us"); + spdlog::info( + "High-priority task interval (curr | avg) = {} | {} us", measured_interval, avg_interval); } void PrintHelp() @@ -101,12 +101,9 @@ void PrintHelp() int main(int argc, char* argv[]) { - // log object for printing message with timestamp and coloring - flexiv::Log log; - // Parse Parameters //============================================================================================== - if (flexiv::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { + if (flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { PrintHelp(); return 1; } @@ -114,12 +111,11 @@ int main(int argc, char* argv[]) try { // Periodic Tasks //========================================================================================== - flexiv::Scheduler scheduler; + flexiv::rdk::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority - scheduler.AddTask( - std::bind(highPriorityTask, std::ref(log)), "HP periodic", 1, scheduler.max_priority()); + scheduler.AddTask(std::bind(highPriorityTask), "HP periodic", 1, scheduler.max_priority()); // Add periodic task with 1s interval and lowest applicable priority - scheduler.AddTask(std::bind(lowPriorityTask, std::ref(log)), "LP periodic", 1000, 0); + scheduler.AddTask(std::bind(lowPriorityTask), "LP periodic", 1000, 0); // Start all added tasks scheduler.Start(); @@ -131,7 +127,7 @@ int main(int argc, char* argv[]) scheduler.Stop(); // Restart scheduler after 2 seconds - log.Warn("Scheduler will restart in 2 seconds"); + spdlog::warn("Scheduler will restart in 2 seconds"); std::this_thread::sleep_for(std::chrono::seconds(2)); g_stop_sched = false; scheduler.Start(); @@ -140,11 +136,11 @@ int main(int argc, char* argv[]) while (!g_stop_sched) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } - // Received signal to stop scheduler tasks, flexiv::Scheduler's destructor can also do the - // thread exit and resources cleanup + // Received signal to stop scheduler tasks, flexiv::rdk::Scheduler's destructor can also do + // the thread exit and resources cleanup } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); return 1; } diff --git a/test/test_timeliness_monitor.cpp b/test/test_timeliness_monitor.cpp index e7bc4c2d..547b72e9 100644 --- a/test/test_timeliness_monitor.cpp +++ b/test/test_timeliness_monitor.cpp @@ -5,14 +5,14 @@ * check to fail. A warning will be issued first, then if the check has failed too many times, the * RDK connection with the server will be closed. During this test, the robot will hold its position * using joint torque streaming mode. - * @copyright Copyright (C) 2016-2023 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -26,8 +26,7 @@ std::atomic g_stop_sched = {false}; } // callback function for realtime periodic task -void PeriodicTask( - flexiv::Robot& robot, flexiv::Log& log, const std::array& init_pos) +void PeriodicTask(flexiv::rdk::Robot& robot, const std::vector& init_pos) { // Loop counter static unsigned int loop_counter = 0; @@ -39,12 +38,12 @@ void PeriodicTask( "PeriodicTask: Fault occurred on the connected robot, exiting ..."); } // Hold position - std::array target_vel = {}; - std::array target_acc = {}; + std::vector target_vel(robot.info().DoF); + std::vector target_acc(robot.info().DoF); robot.StreamJointPosition(init_pos, target_vel, target_acc); if (loop_counter == 5000) { - log.Warn(">>>>> Adding simulated loop delay <<<<<"); + spdlog::warn(">>>>> Adding simulated loop delay <<<<<"); } // simulate prolonged loop time after 5 seconds else if (loop_counter > 5000) { @@ -54,7 +53,7 @@ void PeriodicTask( loop_counter++; } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); g_stop_sched = true; } } @@ -72,12 +71,9 @@ void PrintHelp() int main(int argc, char* argv[]) { - // log object for printing message with timestamp and coloring - flexiv::Log log; - // Parse Parameters //============================================================================================== - if (argc < 2 || flexiv::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { + if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) { PrintHelp(); return 1; } @@ -89,43 +85,42 @@ int main(int argc, char* argv[]) // RDK Initialization //========================================================================================== // Instantiate robot interface - flexiv::Robot robot(robot_sn); + flexiv::rdk::Robot robot(robot_sn); // Clear fault on the connected robot if any if (robot.fault()) { - log.Warn("Fault occurred on the connected robot, trying to clear ..."); + spdlog::warn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault if (!robot.ClearFault()) { - log.Error("Fault cannot be cleared, exiting ..."); + spdlog::error("Fault cannot be cleared, exiting ..."); return 1; } - log.Info("Fault on the connected robot is cleared"); + spdlog::info("Fault on the connected robot is cleared"); } // enable the robot, make sure the E-stop is released before enabling - log.Info("Enabling robot ..."); + spdlog::info("Enabling robot ..."); robot.Enable(); // Wait for the robot to become operational while (!robot.operational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); } - log.Info("Robot is now operational"); + spdlog::info("Robot is now operational"); // set mode after robot is operational - robot.SwitchMode(flexiv::Mode::RT_JOINT_POSITION); + robot.SwitchMode(flexiv::rdk::Mode::RT_JOINT_POSITION); // Set initial joint positions auto init_pos = robot.states().q; - log.Info("Initial joint positions set to: " + flexiv::utility::Arr2Str(init_pos)); - log.Warn(">>>>> Simulated loop delay will be added after 5 seconds <<<<<"); + spdlog::info("Initial joint positions set to: {}", flexiv::rdk::utility::Vec2Str(init_pos)); + spdlog::warn(">>>>> Simulated loop delay will be added after 5 seconds <<<<<"); // Periodic Tasks //========================================================================================== - flexiv::Scheduler scheduler; + flexiv::rdk::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority - scheduler.AddTask( - std::bind(PeriodicTask, std::ref(robot), std::ref(log), std::ref(init_pos)), + scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::ref(init_pos)), "HP periodic", 1, scheduler.max_priority()); // Start all added tasks scheduler.Start(); @@ -138,7 +133,7 @@ int main(int argc, char* argv[]) scheduler.Stop(); } catch (const std::exception& e) { - log.Error(e.what()); + spdlog::error(e.what()); return 1; } diff --git a/thirdparty/build_and_install_dependencies.sh b/thirdparty/build_and_install_dependencies.sh index 41496057..a8cf88ed 100644 --- a/thirdparty/build_and_install_dependencies.sh +++ b/thirdparty/build_and_install_dependencies.sh @@ -33,6 +33,7 @@ mkdir -p cloned && cd cloned # Build and install all dependencies to INSTALL_DIR bash $SCRIPTPATH/scripts/install_eigen.sh $INSTALL_DIR $NUM_JOBS +bash $SCRIPTPATH/scripts/install_spdlog.sh $INSTALL_DIR $NUM_JOBS bash $SCRIPTPATH/scripts/install_tinyxml2.sh $INSTALL_DIR $NUM_JOBS bash $SCRIPTPATH/scripts/install_foonathan_memory.sh $INSTALL_DIR $NUM_JOBS bash $SCRIPTPATH/scripts/install_Fast-CDR.sh $INSTALL_DIR $NUM_JOBS diff --git a/thirdparty/scripts/install_Fast-CDR.sh b/thirdparty/scripts/install_Fast-CDR.sh index c9013831..972a7022 100644 --- a/thirdparty/scripts/install_Fast-CDR.sh +++ b/thirdparty/scripts/install_Fast-CDR.sh @@ -15,6 +15,7 @@ else fi # Use specific version +git fetch -p git checkout v1.0.24 git submodule update --init --recursive diff --git a/thirdparty/scripts/install_Fast-DDS.sh b/thirdparty/scripts/install_Fast-DDS.sh index e268a2a8..26fca396 100644 --- a/thirdparty/scripts/install_Fast-DDS.sh +++ b/thirdparty/scripts/install_Fast-DDS.sh @@ -16,6 +16,7 @@ else fi # Use specific version +git fetch -p git checkout v2.6.7 git submodule update --init --recursive diff --git a/thirdparty/scripts/install_eigen.sh b/thirdparty/scripts/install_eigen.sh index 1c2f3356..d721f438 100644 --- a/thirdparty/scripts/install_eigen.sh +++ b/thirdparty/scripts/install_eigen.sh @@ -15,6 +15,7 @@ else fi # Use specific version +git fetch -p git checkout 3.3.7 git submodule update --init --recursive diff --git a/thirdparty/scripts/install_foonathan_memory.sh b/thirdparty/scripts/install_foonathan_memory.sh index 817c949a..7c5ea554 100644 --- a/thirdparty/scripts/install_foonathan_memory.sh +++ b/thirdparty/scripts/install_foonathan_memory.sh @@ -15,6 +15,7 @@ else fi # Use specific version +git fetch -p git checkout v1.2.1 git submodule update --init --recursive diff --git a/thirdparty/scripts/install_spdlog.sh b/thirdparty/scripts/install_spdlog.sh new file mode 100644 index 00000000..deb41e86 --- /dev/null +++ b/thirdparty/scripts/install_spdlog.sh @@ -0,0 +1,32 @@ +#!/bin/bash +set -e +echo "Installing spdlog" + +# Get install directory and number of parallel build jobs as script arguments +INSTALL_DIR=$1 +NUM_JOBS=$2 + +# Clone source code +if [ ! -d spdlog ] ; then + git clone https://github.com/gabime/spdlog.git + cd spdlog +else + cd spdlog +fi + +# Use specific version +git fetch -p +git checkout v1.14.1 +git submodule update --init --recursive + +# Configure CMake +mkdir -p build && cd build +cmake .. -DCMAKE_BUILD_TYPE=Release \ + -DBUILD_SHARED_LIBS=OFF \ + -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ + -DCMAKE_INSTALL_PREFIX=$INSTALL_DIR \ + +# Build and install +cmake --build . --target install --config Release -j $NUM_JOBS + +echo "Installed spdlog" diff --git a/thirdparty/scripts/install_tinyxml2.sh b/thirdparty/scripts/install_tinyxml2.sh index 373b668c..bc340ac8 100644 --- a/thirdparty/scripts/install_tinyxml2.sh +++ b/thirdparty/scripts/install_tinyxml2.sh @@ -15,6 +15,7 @@ else fi # Use specific version +git fetch -p git checkout 8.0.0 git submodule update --init --recursive